GrxSimulationItem.java
Go to the documentation of this file.
1 // -*- indent-tabs-mode: nil; tab-width: 4; -*-
2 /*
3  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
4  * All rights reserved. This program is made available under the terms of the
5  * Eclipse Public License v1.0 which accompanies this distribution, and is
6  * available at http://www.eclipse.org/legal/epl-v10.html
7  * Contributors:
8  * General Robotix Inc.
9  * National Institute of Advanced Industrial Science and Technology (AIST)
10  */
11 /*
12  * GrxSimulationItem.java
13  *
14  * Copyright (C) 2007 GeneralRobotix, Inc.
15  * All Rights Reserved
16  *
17  */
18 package com.generalrobotix.ui.item;
19 
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;
26 
27 import jp.go.aist.hrp.simulator.BodyInfo;
28 import jp.go.aist.hrp.simulator.ClockGenerator;
29 import jp.go.aist.hrp.simulator.Controller;
30 import jp.go.aist.hrp.simulator.ControllerHelper;
31 import jp.go.aist.hrp.simulator.DynamicsSimulator;
32 import jp.go.aist.hrp.simulator.DynamicsSimulatorFactory;
33 import jp.go.aist.hrp.simulator.DynamicsSimulatorFactoryHelper;
34 import jp.go.aist.hrp.simulator.ExtraJointType;
35 import jp.go.aist.hrp.simulator.SensorStateHolder;
36 import jp.go.aist.hrp.simulator.ViewSimulator;
37 import jp.go.aist.hrp.simulator.ViewSimulatorHelper;
38 import jp.go.aist.hrp.simulator.WorldStateHolder;
39 import jp.go.aist.hrp.simulator.ControllerPackage.ControllerException;
40 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.IntegrateMethod;
41 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.JointDriveMode;
42 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.LinkDataType;
43 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.SensorOption;
44 
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;
55 
72 
73 @SuppressWarnings("serial")
74 public class GrxSimulationItem extends GrxBaseItem {
75  public static final String TITLE = "Simulation";
76  private static final String FORMAT1 = "%8.3f"; //$NON-NLS-1$
77  private static final int WAIT_COUNT_ = 4;
79  private DynamicsSimulator currentDynamics_;
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>();
84  private static ClockGenerator_impl clockGenerator_ = null;
85 
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;
95  private boolean isSimulatingView_;
96  private double viewSimulationStep_=0;
97  //private StartSimulate simulateAction_ = null;
98 
99  private Thread simThread_;
100  private static final int interval_ = 10; //[ms]
101  private Grx3DView view3D;
102 
103  //private static final String FORMAT1 = "%8.3f"; //$NON-NLS-1$
104  private Object lock2_ = new Object();
105 
106  public GrxSimulationItem(String name, GrxPluginManager manager) {
107  super(name, manager);
108  setExclusive(true);
109  setIcon("grxrobot.png");
110  registerCORBA();
111  }
112 
113  public boolean create() {
114  setDbl("totalTime", 20.0); //$NON-NLS-1$
115  setDbl("timeStep", 0.001); //$NON-NLS-1$
116  setDbl("gravity", 9.8); //$NON-NLS-1$
117  setProperty("method","RUNGE_KUTTA"); //$NON-NLS-1$ //$NON-NLS-2$
118  setBool("integrate", true);
119  setBool("viewsimulate", false);
120  setBool("realTime", false);
121  return true;
122  }
123 
128  }
129 
130  private double simTime_ = 0.0;
131 
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;
140  private WorldStateEx wsx_=null;
141 
142  public boolean startSimulation(boolean isInteractive) {
143 
144  if (isExecuting_){
145  GrxDebugUtil.println("[HRP]@startSimulation now executing."); //$NON-NLS-1$
146  return false;
147  }
148 
149  currentWorld_ = manager_.<GrxWorldStateItem>getSelectedItem(GrxWorldStateItem.class, null);
150  if (currentWorld_ == null) {
151  MessageDialog.openError(null, MessageBundle.get("GrxOpenHRPView.dialog.title.Fail"), MessageBundle.get("GrxOpenHRPView.dialog.message.noWorldState")); //$NON-NLS-1$ //$NON-NLS-2$
152  GrxDebugUtil.println("[HRP]@startSimulation there is no world."); //$NON-NLS-1$
153  return false;
154  }
155 
156  isInteractive_ = isInteractive;
157 
158  if (isInteractive_ && currentWorld_.getLogSize() > 0) {
159  boolean ans = MessageDialog.openConfirm(GrxUIPerspectiveFactory.getCurrentShell(), MessageBundle.get("GrxOpenHRPView.dialog.title.start"), MessageBundle.get("GrxOpenHRPView.dialog.message.start0") + MessageBundle.get("GrxOpenHRPView.dialog.message.start1")); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
160  if (ans != true) {
161  return false;
162  }
163  }
164  currentWorld_.clearLog();
165 
166  GrxOpenHRPView openHRPView = (GrxOpenHRPView)manager_.getView( GrxOpenHRPView.class, true );
167  if(openHRPView != null)
168  openHRPView.fixParam();
169 
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_ ){
177  MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(), MessageBundle.get("GrxOpenHRPView.dialog.title.start"), MessageBundle.get("GrxOpenHRPView.dialog.message.errorLogStepTime")); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
178  return false;
179  }
180 
181  try {
182  if (!initDynamicsSimulator()) {
183  MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(),"", MessageBundle.get("GrxOpenHRPView.dialog.message.failedInit")); //$NON-NLS-1$ //$NON-NLS-2$
184  return false;
185  }
186  if (!initController()) {
187  return false;
188  }
189  } catch (Exception e) {
190  GrxDebugUtil.printErr("SimulationLoop:", e); //$NON-NLS-1$
191  return false;
192  }
193 
194  if(isSimulatingView_){
195  view3D = (Grx3DView)manager_.getView( Grx3DView.class, false );
196  if(view3D==null){
197  IWorkbench workbench = PlatformUI.getWorkbench();
198  IWorkbenchWindow window = workbench.getActiveWorkbenchWindow();
199  IWorkbenchPage page = window.getActivePage();
200  try {
201  page.showView("com.generalrobotix.ui.view.Grx3DViewPart", null, IWorkbenchPage.VIEW_CREATE); //$NON-NLS-1$
202  } catch (PartInitException e1) {
203  e1.printStackTrace();
204  }
205  view3D = (Grx3DView)manager_.getView( Grx3DView.class, true );
206  }
207  }
208 
209  if(!isSimulatingView_){
210  GrxLoggerView view = (GrxLoggerView)manager_.getView( GrxLoggerView.class, false );
211  if( view == null){
212  IWorkbench workbench = PlatformUI.getWorkbench();
213  IWorkbenchWindow window = workbench.getActiveWorkbenchWindow();
214  IWorkbenchPage page = window.getActivePage();
215  try {
216  page.showView("com.generalrobotix.ui.view.GrxLoggerViewPart", null, IWorkbenchPage.VIEW_CREATE); //$NON-NLS-1$
217  } catch (PartInitException e1) {
218  e1.printStackTrace();
219  }
220  }
221  }
222  notifyObservers("StartSimulation", isSimulatingView_);
223 
224  clockGenerator_.resetClockReceivers();
225 
226  simTime_ = 0.0;
227  simulateTime_ = 0;
228  currentWorld_.init();
229  simThreadState_ = EXEC;
230  viewSimulationUpdate_ = false;
231  simThread_ = _createSimulationThread();
232  simThread_.start();
233 
234  Runnable run = new Runnable(){
235  public void run() {
236  switch(simThreadState_){
237  case TIMEOVER:
238  if(extendTime())
239  simThreadState_=EXEC;
240  else
241  simThreadState_=STOP;
242  synchronized(lock_){
243  lock_.notifyAll();
244  }
245  case EXEC:
246  if(isSimulatingView_){
247  if(viewSimulationUpdate_){
248  view3D._showCollision(wsx_.collisions);
249  view3D.updateModels(wsx_);
250  view3D.updateViewSimulator(wsx_.time);
251  currentWorld_.setPosition(currentWorld_.getLogSize()-1,view3D);
252  synchronized(lock3_){
253  viewSimulationUpdate_=false;
254  lock3_.notify();
255  }
256  }
257  }
258  Display display = Display.getCurrent();
259  if ( display!=null && !display.isDisposed()){
260  display.timerExec(interval_, this);
261  }
262  break;
263  case STOP:
264  endOfSimulation();
265  synchronized(lock2_){
266  lock2_.notifyAll();
267  }
268  break;
269  case INTERRUPT:
270  MessageDialog.openError( GrxUIPerspectiveFactory.getCurrentShell(),
271  MessageBundle.get("GrxOpenHRPView.dialog.title.Interrupt"), MessageBundle.get("GrxOpenHRPView.dialog.message.Interrupt")); //$NON-NLS-1$ //$NON-NLS-2$
272  endOfSimulation();
273  break;
274  default :
275  break;
276  }
277  }
278  };
279  Display display = Display.getCurrent();
280  if ( display!=null && !display.isDisposed()){
281  display.timerExec(interval_, run);
282  }
283 
284  GrxDebugUtil.println("[OpenHRP]@startSimulation Start Thread and end this function."); //$NON-NLS-1$
285  return true;
286  }
287 
288  String timeMsg_;
289  String updateTimeMsg(){
290  timeMsg_ =
291  MessageBundle.get("GrxOpenHRPView.dialog.message.simTime0") + String.format(FORMAT1, simTime_) + MessageBundle.get("GrxOpenHRPView.dialog.message.simTime1") + //$NON-NLS-1$ //$NON-NLS-2$
292  MessageBundle.get("GrxOpenHRPView.dialog.message.simTime2") + String.format(FORMAT1, simulateTime_) + MessageBundle.get("GrxOpenHRPView.dialog.message.simTime3") + //$NON-NLS-1$ //$NON-NLS-2$
293  MessageBundle.get("GrxOpenHRPView.dialog.message.simTime4") + String.format(FORMAT1, simulateTime_/simTime_); //$NON-NLS-1$
294  return timeMsg_;
295  }
296 
297  private Thread _createSimulationThread(){
298  Thread thread = new Thread(){
299  public void run() {
300  isExecuting_ = true;
301  long suspendT = 0;
302  long startT = System.currentTimeMillis();
303  try {
304  while (isExecuting_) {
305  if (isSuspending_) {
306  long s = System.currentTimeMillis();
307  Thread.sleep(200);
308  suspendT += System.currentTimeMillis() - s;
309  } else {
310  if (!simulateOneStep()){
311  long s = System.currentTimeMillis();
312  synchronized(lock_){
313  simThreadState_ = TIMEOVER;
314  lock_.wait();
315  }
316  suspendT += System.currentTimeMillis() - s;
317  if(simThreadState_==STOP)
318  break;
319  }else{
320  if(isRealTime_){
321  long s = System.currentTimeMillis();
322  long sleep = (long)(simTime_*1000.0) - (s - startT);
323  if(sleep > 0)
324  Thread.sleep(sleep);
325  }
326  }
327  }
328  Thread.yield();
329  }
330  isExecuting_ = false;
331  simulateTime_ += (System.currentTimeMillis() - startT - suspendT)/1000.0;
332 
333  //for (ControllerAttribute i: controllers_) {
334  // i.deactive();
335  //}
336  simThreadState_ = STOP;
337  } catch (Exception e) {
338  GrxDebugUtil.printErr("Simulation Interrupted by Exception:",e); //$NON-NLS-1$
339  isExecuting_ = false;
340  simThreadState_ = INTERRUPT;
341  }
342  }
343  };
344  thread.setPriority(Thread.currentThread().getPriority() - 1);
345  return thread;
346  }
347 
348  public void continueSimulation(){
349  simThread_ = _createSimulationThread();
350  simThread_.start();
351  }
352 
353  public void stopSimulation() {
354  if (isExecuting_)
355  isExecuting_ = false;
356  }
357 
358  public void endOfSimulation(){
359  for (ControllerAttribute i: controllers_) {
360  i.deactive();
361  }
362  updateTimeMsg();
363  System.out.println(new java.util.Date()+timeMsg_.replace(" ", "").replace("\n", " : ")); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$ //$NON-NLS-4$
364  if (isInteractive_) {
365  isInteractive_ = false;
366  execSWT( new Runnable(){
367  public void run(){
368  MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(),
369  MessageBundle.get("GrxOpenHRPView.dialog.title.finish"), timeMsg_); //$NON-NLS-1$
370  }
371  } ,
372  Thread.currentThread() != simThread_
373  );
374  }
375  syncExec(new Runnable(){
376  public void run() {
377  currentWorld_.stopSimulation();
378  notifyObservers("StopSimulation");
379  currentWorld_.setPosition(0);
380  }
381  } );
382  }
383 
388  private boolean simulateOneStep() {
389  if (simTime_ > totalTime_ ) {
390  return false;
391  }
392 
393  // input
394  for (int i = 0; i<controllers_.size(); i++) {
395  ControllerAttribute attr =
396  (ControllerAttribute)controllers_.get(i);
397  attr.input(simTime_);
398  }
399 
400  simTime_ += stepTime_;
401 
402  // control
403  for (int i = 0; i < controllers_.size(); i++) {
404  ControllerAttribute attr = controllers_.get(i);
405  attr.control();
406  }
407  clockGenerator_.updateExecutionContext(simTime_);
408 
409  // simulate
410  if (isIntegrate_) {
411  currentDynamics_.stepSimulation();
412  } else {
413  currentDynamics_.calcWorldForwardKinematics();
414  }
415 
416  // log
417  wsx_=null;
418  if ((simTime_ % logStepTime_) < stepTime_) {
419  currentDynamics_.getWorldState(stateH_);
420  wsx_ = new WorldStateEx(stateH_.value);
421  for (int i=0; i<robotEntry_.size(); i++) {
422  String name = robotEntry_.get(i);
423  currentDynamics_.getCharacterSensorState(name, cStateH_);
424  wsx_.setSensorState(name, cStateH_.value);
425  }
426  if (!isIntegrate_)
427  wsx_.time = simTime_;
428  currentWorld_.addValue(simTime_, wsx_);
429  }
430 
431  // viewSimlulation update
432  if(isSimulatingView_){
433  if ((simTime_ % viewSimulationStep_) < stepTime_) {
434  if(wsx_==null){
435  currentDynamics_.getWorldState(stateH_);
436  wsx_ = new WorldStateEx(stateH_.value);
437  for (int i=0; i<robotEntry_.size(); i++) {
438  String name = robotEntry_.get(i);
439  currentDynamics_.getCharacterSensorState(name, cStateH_);
440  wsx_.setSensorState(name, cStateH_.value);
441  }
442  if (!isIntegrate_)
443  wsx_.time = simTime_;
444  }
445  synchronized(lock3_){
446  try {
447  viewSimulationUpdate_ = true;
448  lock3_.wait();
449  } catch (InterruptedException e) {
450  e.printStackTrace();
451  }
452  }
453  }
454  }
455 
456  // output
457  for (int i = 0; i < controllers_.size(); i++) {
458  ControllerAttribute attr = controllers_.get(i);
459  attr.output();
460  }
461  return true;
462  }
463 
464  public void waitStopSimulation() throws InterruptedException {
465  try {
466  synchronized(lock2_){
467  lock2_.wait();
468  }
469  } catch (InterruptedException e) {
470  stopSimulation();
471  throw e;
472  }
473  }
474 
475  private class ControllerAttribute {
476  String modelName_;
477  String controllerName_;
478  Controller controller_;
479  double stepTime_;
480  int doCount_ = 0;
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;
487  }
488 
489  private void reset(Controller controller, double stepTime) {
490  controller_ = controller;
491  stepTime_ = stepTime;
492  doCount_ = 0;
493  doFlag_ = false;
494  }
495 
496  private void input(double time){
497  try {
498  doFlag_ = false;
499  if (doCount_ <= time/stepTime_) {
500  doFlag_ = true;
501  doCount_++;
502  controller_.input();
503  }
504  } catch (Exception e) {
505  GrxDebugUtil.printErr("Exception in input", e); //$NON-NLS-1$
506  }
507  }
508 
509  private void control() {
510  try {
511  if (doFlag_) controller_.control();
512  } catch (Exception e) {
513  GrxDebugUtil.printErr("Exception in control", e); //$NON-NLS-1$
514  }
515  }
516 
517  private void output() {
518  try {
519  if (doFlag_) controller_.output();
520  } catch (Exception e) {
521  GrxDebugUtil.printErr("Exception in output", e); //$NON-NLS-1$
522  }
523  }
524 
525  private void deactive(){
526  try {
527  controller_.stop();
528  } catch (Exception e) {
529  GrxDebugUtil.printErr("Exception in deactive", e); //$NON-NLS-1$
530  }
531  }
532  private void active() {
533  try {
534  controller_.initialize();
535  } catch(ControllerException e){
536  System.out.println("setupController:"+e.description);
537  MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(), "", MessageBundle.get("GrxOpenHRPView.dialog.message.failedController"));
538  }catch (Exception e) {
539  GrxDebugUtil.printErr("Exception in active", e); //$NON-NLS-1$
540  }
541  }
542  }
543 
544 
545  void execSWT( Runnable r, boolean execInCurrentThread ){
546  if( execInCurrentThread ) {
547  r.run();
548  }else{
549  Display display = Display.getDefault();
550  if ( display!=null && !display.isDisposed())
551  display.asyncExec(r);
552  }
553  }
554 
555  public boolean registerCORBA() {
556  if(clockGenerator_ == null){
557  clockGenerator_ = new ClockGenerator_impl();
559  ClockGenerator cg = clockGenerator_._this(GrxCorbaUtil.getORB());
560  NameComponent[] path = {new NameComponent("ClockGenerator", "")};
561  try {
562  rootnc.rebind(path, cg);
563  GrxDebugUtil.println("OpenHRPView : ClockGenerator is successfully registered to NamingService");
564  } catch (Exception ex) {
565  GrxDebugUtil.println("OpenHRPView : failed to bind ClockGenerator to NamingService");
566  }
567  return true;
568  }else
569  return false;
570  }
571 
572  public void unregisterCORBA() {
574  NameComponent[] path = {new NameComponent("ClockGenerator", "")};
575  try{
576  rootnc.unbind(path);
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");
580  }
581  clockGenerator_ = null;
582  }
583 
584  public void shutdown() {
585  if (currentDynamics_ != null) {
586  try {
587  currentDynamics_.destroy();
588  } catch (Exception e) {
589  GrxDebugUtil.printErr("", e); //$NON-NLS-1$
590  }
591  currentDynamics_ = null;
592  }
593  Collection<GrxSimulationItem> col=(Collection<GrxSimulationItem>) manager_.getItemMap(GrxSimulationItem.class).values();
594  if(col.size()==1 && col.contains(this))
595  unregisterCORBA();
596  }
597 
598  public void delete() {
599  shutdown();
600  super.delete();
601  }
602 
603  public boolean initDynamicsSimulator() {
604  getDynamicsSimulator(true);
605 
606  try {
607  List<GrxModelItem> modelList = manager_.<GrxModelItem>getSelectedItemList(GrxModelItem.class);
608  robotEntry_.clear();
609  float cameraFrameRate = 1;
610  for (int i=0; i<modelList.size(); i++) {
611  GrxModelItem model = modelList.get(i);
612  if (model.links_ == null)
613  continue;
615  return false;
616  BodyInfo bodyInfo = model.getBodyInfo();
617  if(bodyInfo==null) return false;
618  currentWorld_ = manager_.<GrxWorldStateItem>getSelectedItem(GrxWorldStateItem.class, null);
619  if(currentWorld_!=null)
620  currentWorld_.registerCharacter(model.getName(), bodyInfo);
621  currentDynamics_.registerCharacter(model.getName(), bodyInfo);
622  if (model.isRobot()) {
623  robotEntry_.add(model.getName());
624  }
625  List<Camera_impl> cameraList = model.getCameraSequence();
626  for (int j=0; j<cameraList.size(); j++) {
627  Camera_impl camera = cameraList.get(j);
628  float frameRate = camera.getCameraParameter().frameRate;
629  cameraFrameRate = lcm(cameraFrameRate, frameRate);
630  }
631  }
632  viewSimulationStep_ = 1.0d/cameraFrameRate;
633 
634  String smethod = getStr("method");
635  IntegrateMethod m=null;
636  if(smethod.equals(SimulationParameterPanel.METHOD_NAMES[0]))
637  m=IntegrateMethod.RUNGE_KUTTA;
638  else
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) });
642 
643  for (int i=0; i<modelList.size(); i++) {
644  GrxModelItem model = modelList.get(i);
645  if (model.links_ == null)
646  continue;
647 
648  // SET INITIAL ROBOT POSITION AND ATTITUDE
649  GrxLinkItem base = model.rootLink();
650  currentDynamics_.setCharacterLinkData(
651  model.getName(), base.getName(), LinkDataType.ABS_TRANSFORM,
652  model.getInitialTransformArray(base));
653 
655  currentDynamics_.setCharacterLinkData(
656  model.getName(), base.getName(), LinkDataType.ABS_VELOCITY,
657  model.getInitialVelocity(base));
658 
659  // SET I/O MODE OF JOINTS
660  if (isIntegrate_) {
661  double[] jms = model.getInitialJointMode();
662  for (int j=0; j<jms.length; j++) {
663  double[] mode = new double[1];
664  mode[0] = jms[j];
665  currentDynamics_.setCharacterLinkData(model.getName(), model.links_.get(j).getName(), LinkDataType.POSITION_GIVEN, mode );
666  }
667  } else {
668  currentDynamics_.setCharacterAllJointModes(model.getName(), JointDriveMode.HIGH_GAIN_MODE);
669  }
670 
671  // SET INITIAL JOINT VALUES
672  currentDynamics_.setCharacterAllLinkData(
673  model.getName(), LinkDataType.JOINT_VALUE,
674  model.getInitialJointValues());
675  // SET INITIAL JOINT VELOCITY
676  currentDynamics_.setCharacterAllLinkData(
677  model.getName(), LinkDataType.JOINT_VELOCITY,
678  model.getInitialJointVelocity());
679  }
680  currentDynamics_.calcWorldForwardKinematics();
681 
682  // SET COLLISION PAIR
683  List<GrxBaseItem> collisionPair = manager_.getSelectedItemList(GrxCollisionPairItem.class);
684  for (int i=0; i<collisionPair.size(); i++) {
685  GrxCollisionPairItem item = (GrxCollisionPairItem) collisionPair.get(i);
686  currentDynamics_.registerCollisionCheckPair(
687  item.getStr("objectName1", ""), //$NON-NLS-1$ //$NON-NLS-2$
688  item.getStr("jointName1", ""), //$NON-NLS-1$ //$NON-NLS-2$
689  item.getStr("objectName2", ""), //$NON-NLS-1$ //$NON-NLS-2$
690  item.getStr("jointName2", ""), //$NON-NLS-1$ //$NON-NLS-2$
691  item.getDbl("staticFriction", 0.5), //$NON-NLS-1$
692  item.getDbl("slidingFriction", 0.5), //$NON-NLS-1$
693  item.getDblAry("springConstant",new double[]{0.0,0.0,0.0,0.0,0.0,0.0}), //$NON-NLS-1$
694  item.getDblAry("damperConstant",new double[]{0.0,0.0,0.0,0.0,0.0,0.0}), //$NON-NLS-1$
695  item.getDbl("cullingThresh", 0.01), //$NON-NLS-1$
696  item.getDbl("Restitution", 0.0));
697  }
698  // SET Extra Joint
699  List<GrxBaseItem> extraJoints = manager_.getSelectedItemList(GrxExtraJointItem.class);
700  for (int i=0; i<extraJoints.size(); i++) {
701  GrxExtraJointItem item = (GrxExtraJointItem) extraJoints.get(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", ""), //$NON-NLS-1$ //$NON-NLS-2$
711  item.getStr("link1Name", ""), //$NON-NLS-1$ //$NON-NLS-2$
712  item.getStr("object2Name", ""), //$NON-NLS-1$ //$NON-NLS-2$
713  item.getStr("link2Name", ""), //$NON-NLS-1$ //$NON-NLS-2$
714  item.getDblAry("link1LocalPos", new double[]{0.0,0.0,0.0}), //$NON-NLS-1$
715  item.getDblAry("link2LocalPos", new double[]{0.0,0.0,0.0}), //$NON-NLS-1$
716  jointType,
717  item.getDblAry("jointAxis", new double[]{0.0,0.0,0.0}),
718  item.getName());
719  }
720  currentDynamics_.initSimulation();
721 
722  stateH_.value = null;
723  } catch (Exception e) {
724  GrxDebugUtil.printErr("initDynamicsSimulator:", e); //$NON-NLS-1$
725  return false;
726  }
727  return true;
728  }
729 
730  public DynamicsSimulator getDynamicsSimulator(boolean update) {
731  if (update && currentDynamics_ != null) {
732  try {
733  currentDynamics_.destroy();
734  } catch (Exception e) {
735  GrxDebugUtil.printErr("", e); //$NON-NLS-1$
736  }
737  currentDynamics_ = null;
738  }
739 
740  if (currentDynamics_ == null) {
741  try {
742  org.omg.CORBA.Object obj = //process_.get(DynamicsSimulatorID_).getReference();
743  GrxCorbaUtil.getReference("DynamicsSimulatorFactory"); //$NON-NLS-1$
744  DynamicsSimulatorFactory ifactory = DynamicsSimulatorFactoryHelper.narrow(obj);
745  currentDynamics_ = ifactory.create();
746  currentDynamics_._non_existent();
747 
748  } catch (Exception e) {
749  GrxDebugUtil.printErr("getDynamicsSimulator: create failed."); //$NON-NLS-1$
750  e.printStackTrace();
751  currentDynamics_ = null;
752  }
753  }
754  return currentDynamics_;
755  }
756 
757  private boolean initController() {
758  boolean ret = true;
759  List<String> localStrList = new Vector<String>();
760  List<GrxModelItem> modelList = manager_.<GrxModelItem>getSelectedItemList(GrxModelItem.class);
761  for (GrxModelItem model : modelList ) {
762  if( model.isRobot() ){
763  localStrList.add( model.getProperty("controller") ); //$NON-NLS-1$
764  }
765  }
766  _refreshControllers( localStrList );
767  for (GrxModelItem model: modelList ) {
768  if( model.isRobot() ){
769  ret &= _setupController(model, _getControllerFromControllerName(model.getProperty("controller")));//$NON-NLS-1$
770  }
771  }
772  return ret;
773  }
774 
775  private ControllerAttribute _getController(String localID){
777  for (ControllerAttribute i: controllers_) {
778  if ( i.modelName_.equals(localID) ){
779  ret = i;
780  break;
781  }
782  }
783  return ret;
784  }
785 
788  for (ControllerAttribute i: controllers_) {
789  if ( i.controllerName_.equals(controllerName) ){
790  ret = i;
791  break;
792  }
793  }
794  return ret;
795  }
796 
797 
798  private void _refreshControllers(List<String> refStrList){
799  Vector<String> localStrVec = new Vector<String>();
800  for (ControllerAttribute i: controllers_) {
801  int index = refStrList.indexOf(i.controllerName_);
802  if ( index < 0 )
803  localStrVec.add(i.controllerName_);
804  }
805  for (String i: localStrVec) {
806  GrxProcessManager pManager = (GrxProcessManager) manager_.getItem("processManager");
807  AProcess proc = pManager.get(i);
808  if( proc != null && proc.stop() ){
809  pManager.unregister(proc.pi_.id);
810  _getControllerFromControllerName(i);
811  int index = controllers_.indexOf( _getControllerFromControllerName(i) );
812  if ( index >= 0 )
813  controllers_.remove(index);
814  }
815  }
816  }
817 
818  private boolean _setupController(GrxModelItem model, ControllerAttribute deactivatedController) {
819  String controllerName = model.getProperty("controller"); //$NON-NLS-1$
820  if (controllerName == null || controllerName.equals("")) //$NON-NLS-1$
821  return true;
822 
823  double step = model.getDbl("controlTime", 0.005); //$NON-NLS-1$
824  if(stepTime_ > step){
825  MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(), "", MessageBundle.get("GrxOpenHRPView.dialog.message.errorControlTime")); //$NON-NLS-1$ //$NON-NLS-2$
826  return false;
827  }
828 
829  String optionAdd = null;
830  if (!isTrue("integrate", true))
831  optionAdd = " -nosim"; //$NON-NLS-1$
832 
833  GrxDebugUtil.println("model name = " + model.getName() + " : controller = " + controllerName + " : cycle time[s] = " + step); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
834  GrxProcessManager pManager = (GrxProcessManager) manager_.getItem("processManager");;
835 
836  boolean doRestart = false;
837  org.omg.CORBA.Object cobj = GrxCorbaUtil.getReference(controllerName);
838  AProcess proc = pManager.get(controllerName);
839  String dir = model.getStr("setupDirectory", ""); //$NON-NLS-1$ //$NON-NLS-2$
840  String com = model.getStr("setupCommand", ""); //$NON-NLS-1$ //$NON-NLS-2$
841 
842  if (cobj != null) {
843  try {
844  cobj._non_existent();
845  if (isInteractive_ && (!com.equals("") || proc != null)) { // ask only in case being abled to restart process //$NON-NLS-1$
846  MessageDialog dialog =new MessageDialog(GrxUIPerspectiveFactory.getCurrentShell(),MessageBundle.get("GrxOpenHRPView.dialog.title.restartController"),null, //$NON-NLS-1$
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); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$ //$NON-NLS-4$ //$NON-NLS-5$ //$NON-NLS-6$
848  switch( dialog.open() ){
849  case 0: // 0 == "YES"
850  doRestart = true;
851  break;
852  case 1: // 1 == "NO"
853  if(deactivatedController != null)
854  deactivatedController.active();
855  break;
856  default:
857  return false;
858  }
859  }else{
860  if ((!com.equals("") || proc != null) && deactivatedController != null)
861  deactivatedController.active();
862  }
863 
864  } catch (Exception e) {
865  cobj = null;
866  }
867  }
868 
869  if (cobj == null || doRestart) {
870  if (proc != null)
871  proc.stop();
872 
873  if (!com.equals("")) { //$NON-NLS-1$
874  com = dir+java.io.File.separator+com;
875  String osname = System.getProperty("os.name"); //$NON-NLS-1$
876  if(osname.indexOf("Windows") >= 0){ //$NON-NLS-1$
877  com = "\"" + com + "\""; //$NON-NLS-1$ //$NON-NLS-2$
878  }
879  ProcessInfo pi = new ProcessInfo();
880  pi.id = controllerName;
881  pi.dir = dir;
882  pi.com.add(com);
883  pi.waitCount = 2000;
884  pi.isCorbaServer = true;
885  pi.hasShutdown = true;
886  pi.doKillall = false;
887  pi.autoStart = false;
888  pi.autoStop = true;
889  if (proc != null)
890  pManager.unregister(proc.pi_.id);
891  pManager.register(pi);
892  proc = pManager.get(controllerName);
893  }
894 
895  if (proc != null) {
896  GrxDebugUtil.println("Executing controller process ..."); //$NON-NLS-1$
897  GrxDebugUtil.println("dir: " + dir); //$NON-NLS-1$
898  GrxDebugUtil.println("command: " + com); //$NON-NLS-1$
899  proc.start(optionAdd);
900  }
901  }
902 
903  Date before = new Date();
904  for (int j=0; ; j++) {
905  cobj = GrxCorbaUtil.getReference(controllerName);
906  if (cobj != null) {
907  try {
908  Controller controller = ControllerHelper.narrow(cobj);
909  controller.setModelName(model.getName());
910  controller.setDynamicsSimulator(currentDynamics_);
911  if (isTrue("viewsimulate")){//simParamPane_.isSimulatingView()) {
912  cobj = GrxCorbaUtil.getReference("ViewSimulator"); //$NON-NLS-1$
913  ViewSimulator viewsim = ViewSimulatorHelper.narrow(cobj);
914  controller.setViewSimulator(viewsim);
915  }
916  ControllerAttribute refAttr = _getControllerFromControllerName(controllerName);
917  if( refAttr == null){
918  controllers_.add(new ControllerAttribute(model.getName(), controllerName, controller, step));
919  }else{
920  refAttr.reset(controller, step);
921  }
922  GrxDebugUtil.println(" connected to the Controller("+controllerName+")\n"); //$NON-NLS-1$ //$NON-NLS-2$
923  controller.setTimeStep(step);
924  controller.initialize();
925  controller.start();
926  break;
927  }catch (ControllerException e) {
928  System.out.println("setupController:"+e.description);
929  MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(), e.description, MessageBundle.get("GrxOpenHRPView.dialog.message.failedController"));
930  if (proc != null)
931  proc.stop();
932  return false;
933  } catch (Exception e) {
934  GrxDebugUtil.printErr("setupController:", e); //$NON-NLS-1$
935  }
936  }
937 
938  if (j > WAIT_COUNT_ || (new Date().getTime() - before.getTime() > WAIT_COUNT_*1000)) {
939  GrxDebugUtil.println(" failed to setup controller:"+controllerName); //$NON-NLS-1$
940  //タイトル画像をなしにするにはどぁE��れ�EぁE��のか?とりあえずnullにしてみ�
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); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$ //$NON-NLS-4$ //$NON-NLS-5$ //$NON-NLS-6$ //$NON-NLS-7$
942  int ans = dialog.open();
943  if (ans == 0) {
944  before = new Date();
945  j=0;
946  } else if (ans == 1) {
947  MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(), "", MessageBundle.get("GrxOpenHRPView.dialog.message.failedController"));
948  return false;
949  } else {
950  MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(), "", MessageBundle.get("GrxOpenHRPView.dialog.message.failedController"));
951  return false;
952  }
953  } else {
954  try {Thread.sleep(1000);} catch (Exception e) {}
955  }
956  }
957 
958  return true;
959  }
960 
961  private boolean extendTime() {
962  if(!isInteractive_)
963  return false;
964  boolean state = MessageDialog.openQuestion(GrxUIPerspectiveFactory.getCurrentShell(), MessageBundle.get("GrxOpenHRPView.dialog.title.timeUp"), MessageBundle.get("GrxOpenHRPView.dialog.message.TimeUp")); //$NON-NLS-1$ //$NON-NLS-2$
965  if (state == true)
966  return false;
967 
968  String str = null;
969  while (true) {
970  InputDialog dialog = new InputDialog(GrxUIPerspectiveFactory.getCurrentShell(),MessageBundle.get("GrxOpenHRPView.dialog.title.ExtendTime"),MessageBundle.get("GrxOpenHRPView.dialog.message.extendTime"),"5.0",null); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
971  int result = dialog.open();
972  str = dialog.getValue();
973  if (result == InputDialog.CANCEL)
974  return false;
975  try {
976  double d = Double.parseDouble(str);
977  if (d > 0) {
978  //simParamPane_.setTotalTime(simParamPane_.getTotalTime() + d);
979  totalTime_ = totalTime_ + d;
980  setDbl("totalTime", totalTime_);
981  currentWorld_.extendTime(totalTime_);
982  break;
983  }
984  } catch (NumberFormatException e) {}
985  }
986  return true;
987  }
988 
989  public boolean isSimulating(){
990  return isExecuting_;
991  }
992 
993  public void restoreProperties() {
994  super.restoreProperties();
995  String str = getProperty("totalTime");
996  if(str==null)
997  setDbl("totalTime", 20.0); //$NON-NLS-1$
998  str = getProperty("timeStep");
999  if(str==null)
1000  setDbl("timeStep", 0.001); //$NON-NLS-1$
1001  str = getProperty("gravity");
1002  if(str==null)
1003  setDbl("gravity", 9.8); //$NON-NLS-1$
1004  str = getProperty("method");
1005  if(str==null)
1006  setProperty("method","RUNGE_KUTTA"); //$NON-NLS-1$ //$NON-NLS-2$
1007  str = getProperty("integrate");
1008  if(str==null)
1009  setBool("integrate", true);
1010  str = getProperty("realTime");
1011  if(str==null)
1012  setBool("realTime", false);
1013  str = getProperty("viewsimulate");
1014  if(str==null)
1015  setBool("viewsimulate", false);
1016  }
1017 
1018  public ValueEditType GetValueEditType(String key) {
1019  if(key.equals("method")){
1020  return new ValueEditCombo(methodComboItem_);
1021  }else if(key.equals("integrate") || key.equals("viewsimulate") || key.equals("realTime")){
1022  return new ValueEditCombo(booleanComboItem_);
1023  }
1024  return super.GetValueEditType(key);
1025  }
1026 
1027  public int gcd(int a, int b){
1028  BigInteger bigA = BigInteger.valueOf(a);
1029  BigInteger bigB = BigInteger.valueOf(b);
1030  return bigA.gcd(bigB).intValue();
1031  }
1032 
1033  public int lcm(int a, int b){
1034  return a*b/gcd(a,b);
1035  }
1036 
1037  public float lcm(float a, float b){
1038  return (float)lcm((int)a, (int)b);
1039  }
1040 
1041 }
rootnc
Definition: hrp.py:10
void setSensorState(String charName, SensorState state)
void registerCharacter(String cname, BodyInfo binfo)
static final String get(String key)
void updateModels(WorldStateEx state)
state
unsigned int sleep(unsigned int seconds)
final double[] getDblAry(String key, double[] defaultVal)
get double array associated to key
#define null
our own NULL pointer
Definition: IceTypes.h:57
double[] getInitialTransformArray(GrxLinkItem link)
DynamicsSimulator getDynamicsSimulator(boolean update)
png_infop png_charpp name
Definition: png.h:2382
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
item corresponds to a robot model
png_uint_32 i
Definition: png.h:2735
long b
Definition: jpegint.h:371
void _showCollision(Collision[] collisions)
static org.omg.CORBA.ORB getORB(String[] argv)
initialize and get ORB
boolean _setupController(GrxModelItem model, ControllerAttribute deactivatedController)
GrxLinkItem rootLink()
get root link
def j(str, encoding="cp932")
list index
def run(tree, args)
ControllerAttribute _getController(String localID)
boolean startSimulation(boolean isInteractive)
path
final Double getDbl(String key, Double defaultVal)
get double value associated to key
double[] getInitialVelocity(GrxLinkItem link)
static org.omg.CORBA.Object getReference(String id)
get CORBA object which is associated with id
final String getStr(String key)
get value associated to keyword
プラグイン管理クラス GrxUIの核になるクラス。プラグインのロード等の、初期化を実行する。 プラグインとそ...
void _refreshControllers(List< String > refStrList)
org
static NamingContext getNamingContext()
get naming context
NamingContext
Definition: hrpPrep.py:134
List< Camera_impl > getCameraSequence()
get sequence of cameras
ControllerAttribute _getControllerFromControllerName(String controllerName)
NameComponent
Definition: hrpPrep.py:128
GrxSimulationItem(String name, GrxPluginManager manager)


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:38