00001 package com.generalrobotix.ui.view;
00002
00003 import java.util.ArrayList;
00004 import java.util.Date;
00005
00006 import jp.go.aist.hrp.simulator.DynamicsSimulator;
00007 import jp.go.aist.hrp.simulator.DynamicsSimulatorFactory;
00008 import jp.go.aist.hrp.simulator.DynamicsSimulatorFactoryHelper;
00009 import jp.go.aist.hrp.simulator.SensorState;
00010 import jp.go.aist.hrp.simulator.WorldStateHolder;
00011 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.IntegrateMethod;
00012 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.LinkDataType;
00013 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.SensorOption;
00014
00015 import org.eclipse.jface.dialogs.MessageDialog;
00016 import org.eclipse.swt.SWT;
00017 import org.eclipse.swt.events.KeyAdapter;
00018 import org.eclipse.swt.events.KeyEvent;
00019 import org.eclipse.swt.events.SelectionEvent;
00020 import org.eclipse.swt.events.SelectionListener;
00021 import org.eclipse.swt.graphics.Image;
00022 import org.eclipse.swt.layout.GridData;
00023 import org.eclipse.swt.layout.GridLayout;
00024 import org.eclipse.swt.widgets.Button;
00025 import org.eclipse.swt.widgets.Composite;
00026 import org.eclipse.swt.widgets.Display;
00027 import org.eclipse.swt.widgets.Label;
00028 import org.eclipse.swt.widgets.Text;
00029
00030 import OpenHRP.RobotHardwareService;
00031 import OpenHRP.RobotHardwareServiceHelper;
00032 import OpenHRP.StateHolderService;
00033 import OpenHRP.StateHolderServiceHelper;
00034 import OpenHRP.StateHolderServicePackage.CommandHolder;
00035 import OpenHRP.RobotHardwareServicePackage.RobotStateHolder;
00036 import OpenHRP.RobotHardwareServicePackage.SwitchStatus;
00037 import RTC.ComponentProfile;
00038 import RTC.ConnectorProfile;
00039 import RTC.ConnectorProfileHolder;
00040 import RTC.ExecutionContext;
00041 import RTC.LifeCycleState;
00042 import RTC.PortInterfaceProfile;
00043 import RTC.PortProfile;
00044 import RTC.PortService;
00045 import RTC.RTObject;
00046 import RTC.RTObjectHelper;
00047
00048 import com.generalrobotix.ui.GrxBaseItem;
00049 import com.generalrobotix.ui.GrxBaseView;
00050 import com.generalrobotix.ui.GrxBaseViewPart;
00051 import com.generalrobotix.ui.GrxPluginManager;
00052 import com.generalrobotix.ui.grxui.Activator;
00053 import com.generalrobotix.ui.item.GrxModelItem;
00054 import com.generalrobotix.ui.item.GrxWorldStateItem;
00055 import com.generalrobotix.ui.item.GrxWorldStateItem.WorldStateEx;
00056 import com.generalrobotix.ui.util.GrxCorbaUtil;
00057 import com.generalrobotix.ui.util.GrxDebugUtil;
00058 import com.generalrobotix.ui.util.GrxXmlUtil;
00059
00060 @SuppressWarnings("serial")
00064 public class GrxRobotHardwareClientView extends GrxBaseView {
00065 public static final String TITLE = "RobotHardware RTC Client";
00066 private static final int NOT_CONNECTED = 0;
00067 private static final int CONNECTING = 1;
00068 private static final int CONNECTED = 2;
00069
00070 private GrxJythonPromptView jythonView_;
00071
00072 private StateHolderService sholder_;
00073 private RobotHardwareService hwCtrl_;
00074
00075 private DynamicsSimulator dynamics_;
00076 private GrxWorldStateItem currentItem_;
00077 private GrxModelItem currentModel_;
00078
00079 private WorldStateHolder worldStateH_ = new WorldStateHolder();
00080 private RobotStateHolder robotStateH_ = new RobotStateHolder();
00081
00082 private String robotType_ = "-----";
00083 private String robotHost_ = "localhost";
00084 private int robotPort_ = 2809;
00085 private int interval_ = 200;
00086 private String setupFile_ = "-----";
00087 private String RobotHardwareRTC_ = "RobotHardware0";
00088 private String StateHolderRTC_ = "StateHolder0";
00089
00090 private Date initialDate_;
00091 private Date prevDate_;
00092 private int state_ = NOT_CONNECTED;
00093 private boolean actualFlag = false;
00094 private Image startMonitorIcon_ = Activator.getDefault().getImage("sim_start.png");
00095 private Image stopMonitorIcon_ = Activator.getDefault().getImage("sim_stop.png");
00096 private Image servoOnIcon_ = Activator.getDefault().getImage("robot_servo_start.png");
00097 private Image servoOffIcon_ = Activator.getDefault().getImage("robot_servo_stop.png");
00098 private Image lampOnIcon_ = Activator.getDefault().getImage("lamp_on.png");
00099 private Image lampOffIcon_ = Activator.getDefault().getImage("lamp_off.png");
00100 private Button btnConnect_;
00101 private Button btnServo_;
00102 private Button btnSetup_;
00103 private Label lblLamp_;
00104 private Label lblStatus_;
00105 private Text fldStatus_;
00106 private Composite propertyPanel_;
00107 private boolean isMonitorRunning_;
00108
00109 private ArrayList<SetPropertyPanel> propList_ = new ArrayList<SetPropertyPanel>();
00110
00118 public GrxRobotHardwareClientView(String name, GrxPluginManager manager, GrxBaseViewPart vp, Composite parent) {
00119 super(name, manager, vp, parent);
00120
00121 composite_.setLayout(new GridLayout(1,false));
00122 Composite layout1 = new Composite(composite_, SWT.NONE);
00123 layout1.setLayoutData(new GridData(GridData.HORIZONTAL_ALIGN_CENTER));
00124 GridLayout gridLayout = new GridLayout(6, false);
00125 gridLayout.marginWidth = 5;
00126 gridLayout.horizontalSpacing = 5;
00127 layout1.setLayout(gridLayout);
00128
00129 btnServo_ = new Button(layout1, SWT.TOGGLE);
00130 btnServo_.setImage(servoOnIcon_);
00131 GridData btnGridData = new GridData();
00132 btnGridData.widthHint = 32;
00133 btnGridData.heightHint = 32;
00134 btnServo_.setLayoutData(btnGridData);
00135
00136 lblStatus_ = new Label(layout1, SWT.NONE);
00137 lblStatus_.setText("Status");
00138
00139 lblLamp_ = new Label(layout1, SWT.NONE);
00140 lblLamp_.setImage(lampOffIcon_);
00141
00142 fldStatus_ = new Text(layout1, SWT.SINGLE | SWT.BORDER);
00143 fldStatus_.setText("Not Connected.");
00144 GridData textGridData = new GridData();
00145 textGridData.widthHint = 100;
00146 fldStatus_.setLayoutData(textGridData);
00147
00148 btnConnect_ = new Button(layout1, SWT.TOGGLE);
00149 btnConnect_.setLayoutData(btnGridData);
00150 btnConnect_.setImage(startMonitorIcon_);
00151
00152 btnSetup_ = new Button(layout1, SWT.NONE);
00153 btnSetup_.setText("ROBOT SETUP");
00154
00155 propertyPanel_ = new Composite(composite_,SWT.NONE);
00156 propertyPanel_.setLayoutData(new GridData(GridData.HORIZONTAL_ALIGN_CENTER));
00157 GridLayout gridLayout1 = new GridLayout(1, false);
00158 gridLayout.marginWidth = 5;
00159 gridLayout.horizontalSpacing = 5;
00160 propertyPanel_.setLayout(gridLayout1);
00161 propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "Robot Host", "robotHost", true, robotHost_));
00162 propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "Robot Port", "robotPort", true, new Integer(robotPort_).toString()));
00163 propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "Robot Name", "ROBOT", true, robotType_));
00164 propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "Interval[ms]","interval", true, new Integer(interval_).toString()));
00165 propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "Setup File", "setupFile", true, setupFile_));
00166 propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "RobotHardwareService", "RobotHardwareServiceRTC", true, RobotHardwareRTC_));
00167 propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "StateHolderService", "StateHolderRTC", true, StateHolderRTC_));
00168
00169 btnConnect_.addSelectionListener(new SelectionListener() {
00170 public void widgetDefaultSelected(SelectionEvent e) {}
00171 public void widgetSelected(SelectionEvent e) {
00172 if (btnConnect_.getSelection()) {
00173 startMonitor(true);
00174 } else {
00175 boolean ans = MessageDialog.openConfirm(getParent().getShell(),
00176 "Do you really want to stop monitoring?",
00177 "Stopping Robot State Monitor");
00178
00179 if (ans){
00180 stopMonitor();
00181 }else{
00182 btnConnect_.setSelection(true);
00183 }
00184 }
00185 }
00186 });
00187
00188 btnSetup_.setEnabled(false);
00189 btnSetup_.addSelectionListener(new SelectionListener() {
00190 public void widgetDefaultSelected(SelectionEvent e) {}
00191 public void widgetSelected(SelectionEvent e) {
00192 setupFile_ = getProperty("setupFile");
00193 getJythonView().execFile(GrxXmlUtil.expandEnvVal(setupFile_));
00194 }
00195 });
00196
00197 btnServo_.setEnabled(false);
00198 btnServo_.addSelectionListener(new SelectionListener() {
00199 public void widgetDefaultSelected(SelectionEvent e) {}
00200 public void widgetSelected(SelectionEvent e) {
00201 if (btnServo_.getSelection()){
00202 servoOn();
00203 }else{
00204 servoOff();
00205 }
00206 }
00207 });
00208
00209 currentItem_ = manager_.<GrxWorldStateItem>getSelectedItem(GrxWorldStateItem.class, null);
00210 if(currentItem_!=null){
00211 currentItem_.addObserver(this);
00212 }
00213 manager_.registerItemChangeListener(this, GrxWorldStateItem.class);
00214 }
00215
00216 public void registerItemChange(GrxBaseItem item, int event){
00217 if(item instanceof GrxWorldStateItem){
00218 GrxWorldStateItem worldStateItem = (GrxWorldStateItem) item;
00219 switch(event){
00220 case GrxPluginManager.SELECTED_ITEM:
00221 if(currentItem_!=worldStateItem){
00222 currentItem_ = worldStateItem;
00223 currentItem_.addObserver(this);
00224 }
00225 break;
00226 case GrxPluginManager.REMOVE_ITEM:
00227 case GrxPluginManager.NOTSELECTED_ITEM:
00228 if(currentItem_==worldStateItem){
00229 currentItem_.deleteObserver(this);
00230 currentItem_ = null;
00231 }
00232 break;
00233 default:
00234 break;
00235 }
00236 }
00237 }
00238
00239 private class SetPropertyPanel extends Composite {
00240 private String propName;
00241 private boolean isLocal = true;
00242 private String defaultVal;
00243
00244 private Label label;
00245 private Text fld;
00246 private Button set;
00247 private Button resume;
00248
00249
00250 public SetPropertyPanel(Composite parent, int style, String _title, String _propName, boolean _isLocal, String _defaultVal) {
00251 super(parent, style);
00252 GridLayout gridLayout = new GridLayout(4, false);
00253 gridLayout.marginWidth = 5;
00254 gridLayout.horizontalSpacing = 5;
00255 this.setLayout(gridLayout);
00256 GridData labelGridData = new GridData();
00257 labelGridData.widthHint = 150;
00258 label = new Label(this, SWT.NONE);
00259 label.setText(_title);
00260 label.setLayoutData(labelGridData);
00261 fld = new Text(this, SWT.NONE);
00262 GridData textGridData = new GridData();
00263 textGridData.widthHint = 120;
00264 fld.setLayoutData(textGridData);
00265 set = new Button(this, SWT.NONE);
00266 set.setText("Set");
00267 resume = new Button(this, SWT.NONE);
00268 resume.setText("Resume");
00269 propName = _propName;
00270 isLocal = _isLocal;
00271 defaultVal = _defaultVal;
00272
00273 fld.addKeyListener(new KeyAdapter() {
00274 public void keyReleased(KeyEvent e){
00275 if (e.keyCode == SWT.CR) {
00276 set();
00277 } else {
00278 boolean hasChanged = !fld.getText().equals(getValue());
00279 set.setEnabled(hasChanged);
00280 resume.setEnabled(hasChanged);
00281 }
00282 }
00283 });
00284
00285 set.setEnabled(false);
00286 set.addSelectionListener(new SelectionListener() {
00287 public void widgetDefaultSelected(SelectionEvent e) {
00288 }
00289 public void widgetSelected(SelectionEvent e) {
00290 set();
00291 }
00292 });
00293
00294 resume.setEnabled(false);
00295 resume.addSelectionListener(new SelectionListener() {
00296 public void widgetDefaultSelected(SelectionEvent e) {
00297 }
00298 public void widgetSelected(SelectionEvent e) {
00299 resume();
00300 }
00301 });
00302 }
00303
00304 public void setEditable(boolean isEditable) {
00305 fld.setEditable(isEditable);
00306 resume.setEnabled(isEditable);
00307 set.setEnabled(isEditable);
00308 }
00309
00310 public String getValue() {
00311 String str;
00312 if (isLocal)
00313 str = getProperty(propName);
00314 else
00315 str = manager_.getProjectProperty(propName);
00316 if (str == null)
00317 str = defaultVal;
00318 return str;
00319 }
00320
00321 public void set() {
00322 String value = fld.getText();
00323 if (isLocal){
00324 setProperty(propName, value);
00325 }else{
00326 manager_.setProjectProperty(propName, value);
00327 System.out.println("("+propName+","+value+")");
00328 }
00329 set.setEnabled(false);
00330 resume.setEnabled(false);
00331 }
00332
00333 public void resume() {
00334 fld.setText(getValue());
00335 set.setEnabled(false);
00336 resume.setEnabled(false);
00337 }
00338 }
00339
00340 private void startMonitor(final boolean isInteractive) {
00341 if (isMonitorRunning_) return;
00342
00343 isMonitorRunning_ = true;
00344 btnConnect_.setImage(stopMonitorIcon_);
00345 btnConnect_.setToolTipText("Stop Robot State Monitor");
00346 btnConnect_.setSelection(true);
00347
00348 setConnectionState(CONNECTING);
00349
00350
00351
00352 Runnable runnable = new Runnable() {
00353 public void run(){
00354 Display display = composite_.getDisplay();
00355 if (state_ == CONNECTING){
00356 tryConnection(isInteractive);
00357 if (!display.isDisposed())
00358 display.timerExec(1000, this);
00359 }else if (state_ == CONNECTED){
00360 try{
00361 updateRobotState();
00362 }catch(Exception ex){
00363 ex.printStackTrace();
00364 }
00365 interval_ = getInt("interval", 200);
00366 if (!display.isDisposed())
00367 display.timerExec(interval_, this);
00368 }else{
00369 }
00370 }
00371 };
00372 Display display = composite_.getDisplay();
00373 if (!display.isDisposed()){
00374 display.timerExec(1, runnable);
00375 }
00376
00377
00378 }
00379
00380 private void tryConnection(boolean isInteractive){
00381 System.out.println("tryConnection()");
00382 if (currentItem_ == null) return;
00383 try {
00384 for (int i=0; i<propList_.size(); i++)
00385 propList_.get(i).resume();
00386
00387
00388 robotHost_ = getStr("robotHost");
00389 if (robotHost_ != null){
00390 manager_.setProjectProperty("nsHost", robotHost_);
00391 }
00392 try {
00393 robotPort_ = Integer.parseInt(manager_.getProjectProperty("robotPort"));
00394 } catch (Exception e) {
00395 robotPort_ = 2809;
00396 }
00397
00398 robotType_ = getStr("ROBOT");
00399
00400 currentModel_ = (GrxModelItem) manager_.getItem(GrxModelItem.class, robotType_);
00401 if (currentModel_ == null){
00402 System.out.println("Can't find robot model("+robotType_+")");
00403 return;
00404 }
00405
00406
00407
00408
00409 currentItem_.clearLog();
00410 currentItem_.registerCharacter(robotType_, currentModel_.getBodyInfo());
00411
00412
00413 System.out.println("RobotHardwareRTC_ = "+RobotHardwareRTC_);
00414 RTObject rtc = findRTC(RobotHardwareRTC_);
00415 ExecutionContext ec = rtc.get_owned_contexts()[0];
00416 if (ec.get_component_state(rtc) != LifeCycleState.ACTIVE_STATE){
00417 ec.activate_component(rtc);
00418 }
00419 hwCtrl_ = RobotHardwareServiceHelper.narrow(findService(rtc, "service0"));
00420
00421
00422 if (isAnyServoOn()) {
00423 btnServo_.setSelection(true);
00424 btnServo_.setImage(servoOffIcon_);
00425 btnServo_.setToolTipText("Servo Off");
00426 } else {
00427 btnServo_.setSelection(false);
00428 btnServo_.setImage(servoOnIcon_);
00429 btnServo_.setToolTipText("Servo On");
00430 }
00431 prevDate_ = new Date();
00432
00433 initDynamicsSimulator(false);
00434
00435 setConnectionState(CONNECTED);
00436
00437 } catch (Exception e) {
00438
00439
00440 if (isInteractive && currentModel_ == null) {
00441 MessageDialog.openInformation(getParent().getShell(),
00442 "Load Model(" + robotType_ + ") first.",
00443 "Start Robot State Monitor");
00444 stopMonitor();
00445 } else {
00446 setConnectionState(CONNECTING);
00447 }
00448 }
00449 }
00450
00451 private void stopMonitor() {
00452 btnConnect_.setImage(startMonitorIcon_);
00453 btnConnect_.setToolTipText("Start Robot State Monitor");
00454 btnConnect_.setSelection(false);
00455
00456 setConnectionState(NOT_CONNECTED);
00457 isMonitorRunning_ = false;
00458 }
00459
00460 private void setConnectionState(int state) {
00461 switch (state) {
00462 case NOT_CONNECTED:
00463 btnSetup_.setEnabled(false);
00464 btnServo_.setEnabled(false);
00465 fldStatus_.setText("Not Connected.");
00466 lblLamp_.setImage(lampOffIcon_);
00467
00468 for (int i=0; i<propList_.size(); i++) {
00469 propList_.get(i).set.setEnabled(false);
00470 propList_.get(i).resume.setEnabled(false);
00471 }
00472
00473 break;
00474
00475 case CONNECTING:
00476 btnSetup_.setEnabled(false);
00477 btnServo_.setEnabled(false);
00478 fldStatus_.setText("Connecting ...");
00479
00480 break;
00481
00482 case CONNECTED:
00483 btnSetup_.setEnabled(true);
00484 btnServo_.setEnabled(true);
00485 fldStatus_.setText("Connected.");
00486 lblLamp_.setImage(lampOnIcon_);
00487 break;
00488 }
00489 state_ = state;
00490 }
00491
00492 public void restoreProperties() {
00493 super.restoreProperties();
00494 robotHost_ = getStr("robotHost");
00495 if (robotHost_ != null){
00496 manager_.setProjectProperty("nsHost", robotHost_);
00497
00498 }
00499 startMonitor(false);
00500 }
00501
00502 public RTObject findRTC(String name){
00503 org.omg.CORBA.Object obj = GrxCorbaUtil.getReference(name, "rtc", robotHost_, robotPort_);
00504 return RTObjectHelper.narrow(obj);
00505 }
00506
00507 public org.omg.CORBA.Object findService(RTObject rtc, String service){
00508 ComponentProfile prof = rtc.get_component_profile();
00509 PortProfile[] port_prof = prof.port_profiles;
00510 PortService port = null;
00511 for (int i=0; i<port_prof.length; i++){
00512 PortProfile pp = port_prof[i];
00513 PortInterfaceProfile[] ifs = pp.interfaces;
00514 for (int j=0; j<ifs.length; j++){
00515 PortInterfaceProfile aif = ifs[j];
00516 if (aif.instance_name.equals(service)){
00517 port = pp.port_ref;
00518 }
00519 }
00520 }
00521 ConnectorProfile con_prof = new ConnectorProfile();
00522 con_prof.name = "noname";
00523 con_prof.connector_id = "";
00524 con_prof.ports = new PortService[1];
00525 con_prof.ports[0] = port;
00526 con_prof.properties = new _SDOPackage.NameValue[0];
00527 ConnectorProfileHolder con_prof_holder = new ConnectorProfileHolder();
00528 con_prof_holder.value = con_prof;
00529 port.connect(con_prof_holder);
00530 String ior = con_prof_holder.value.properties[0].value.extract_string();
00531 port.disconnect(con_prof_holder.value.connector_id);
00532 return GrxCorbaUtil.getORB().string_to_object(ior);
00533 }
00534
00535
00536 private void updateRobotState(){
00537
00538 if (currentItem_ == null) {
00539 stopMonitor();
00540 return;
00541 }
00542
00543 Date now = new Date();
00544 long time = now.getTime();
00545 if (prevDate_ != null) {
00546 time -= prevDate_.getTime();
00547 }
00548 if (state_ == CONNECTING) {
00549 if (time > 1000) {
00550 prevDate_ = now;
00551 if (lblLamp_.getImage() == lampOffIcon_)
00552 lblLamp_.setImage(lampOnIcon_);
00553 else
00554 lblLamp_.setImage(lampOffIcon_);
00555 startMonitor(false);
00556 }
00557 return;
00558 }
00559
00560 if (state_ == CONNECTED && time > interval_) {
00561 prevDate_ = now;
00562 try {
00563 if (hwCtrl_ != null){
00564 hwCtrl_.getStatus(robotStateH_);
00565 }
00566
00567 if(actualFlag) {
00568 dynamics_.setCharacterAllLinkData(robotType_, LinkDataType.JOINT_VALUE, robotStateH_.value.angle);
00569 } else {
00570
00571 }
00572
00573 try{
00574 if (sholder_ == null){
00575 StateHolderRTC_ = getStr("StateHolderRTC", StateHolderRTC_);
00576 RTObject rtc = findRTC(StateHolderRTC_);
00577 sholder_ = StateHolderServiceHelper.narrow(findService(rtc, "service0"));
00578 }
00579 CommandHolder com = new CommandHolder();
00580 sholder_.getCommand(com);
00581 dynamics_.setCharacterLinkData(robotType_, currentModel_.rootLink().getName(), LinkDataType.ABS_TRANSFORM, com.value.baseTransform);
00582 dynamics_.setCharacterAllLinkData(robotType_, LinkDataType.JOINT_VALUE, robotStateH_.value.command);
00583 }catch(Exception e){
00584 GrxDebugUtil.printErr("failed to connect StateHolderService");
00585 sholder_ = null;
00586 }
00587
00588 dynamics_.calcWorldForwardKinematics();
00589 dynamics_.getWorldState(worldStateH_);
00590
00591 } catch (Exception e) {
00592 GrxDebugUtil.printErr("iobclient got exception:", e);
00593 setConnectionState(CONNECTING);
00594 return;
00595 }
00596
00597 SensorState ss = new SensorState();
00598 ss.q = robotStateH_.value.angle;
00599 ss.u = robotStateH_.value.torque;
00600 ss.force = robotStateH_.value.force;
00601 ss.accel = robotStateH_.value.accel;
00602 ss.rateGyro = robotStateH_.value.rateGyro;
00603
00604 WorldStateEx wsx = new WorldStateEx(worldStateH_.value);
00605 if (currentItem_.getLogSize() == 0){
00606 initialDate_ = now;
00607 }
00608 wsx.time = (now.getTime() - initialDate_.getTime()) / 1000.0;
00609 wsx.collisions = null;
00610 wsx.setSensorState(robotType_, ss);
00611 wsx.setTargetState(robotType_, robotStateH_.value.command);
00612 int [] sstate = new int[robotStateH_.value.servoState.length];
00613 for (int i=0; i<sstate.length; i++){
00614 sstate[i] = robotStateH_.value.servoState[i][0];
00615 }
00616 wsx.setServoState(robotType_, sstate);
00617 wsx.setPowerState(robotType_, robotStateH_.value.voltage, robotStateH_.value.current);
00618 currentItem_.addValue(wsx.time, wsx);
00619 currentItem_.setPosition(currentItem_.getLogSize()-1);
00620 }
00621 }
00622
00623 private DynamicsSimulator initDynamicsSimulator(boolean update) {
00624 if (dynamics_ != null && !update) {
00625 return dynamics_;
00626 }
00627 org.omg.CORBA.Object obj = GrxCorbaUtil.getReference("DynamicsSimulatorFactory");
00628 DynamicsSimulatorFactory dynFactory = DynamicsSimulatorFactoryHelper.narrow(obj);
00629 if (dynamics_ != null) {
00630 try {
00631 dynamics_.destroy();
00632 } catch(Exception e) {
00633 GrxDebugUtil.printErr("", e);
00634 }
00635 }
00636 try {
00637 dynamics_ = dynFactory.create();
00638 dynamics_.registerCharacter(robotType_, currentModel_.getBodyInfo());
00639 dynamics_.init(0.001, IntegrateMethod.EULER, SensorOption.DISABLE_SENSOR);
00640 } catch (Exception e) {
00641 dynamics_ = null;
00642 e.printStackTrace();
00643 }
00644
00645 return dynamics_;
00646 }
00647
00648 private GrxJythonPromptView getJythonView() {
00649 if (jythonView_ == null) {
00650 jythonView_= (GrxJythonPromptView)manager_.getView(GrxJythonPromptView.class,true);
00651 }
00652
00653 return jythonView_;
00654 }
00655
00656 private void servoOn() {
00657 if (isAnyServoOn()) {
00658 return;
00659 }
00660 if(hwCtrl_ != null) {
00661 hwCtrl_.power("all", SwitchStatus.SWITCH_ON);
00662 try{
00663 Thread.sleep(1000);
00664 }catch(Exception ex){
00665 ex.printStackTrace();
00666 }
00667 }
00668
00669 boolean ans = MessageDialog.openConfirm(getParent().getShell(),
00670 "!! Robot Motion Warning (SERVO ON) !!\n" +
00671 "Confirm RELAY turned ON.\n" +
00672 "Then Push [OK] to servo ON.\n",
00673 "Servo ON");
00674 if (ans) {
00675 try {
00676
00677 StateHolderRTC_ = getStr("StateHolderRTC", StateHolderRTC_);
00678 RTObject rtc = findRTC(StateHolderRTC_);
00679 sholder_ = StateHolderServiceHelper.narrow(findService(rtc, "service0"));
00680
00681 if (sholder_ != null) {
00682 sholder_.goActual();
00683 try{
00684 Thread.sleep(100);
00685 }catch(Exception ex){
00686 ex.printStackTrace();
00687 }
00688 }
00689
00690 if(hwCtrl_ != null){
00691 hwCtrl_.servo("all", SwitchStatus.SWITCH_ON);
00692 try {
00693 Thread.sleep(5000);
00694 }catch(Exception ex){
00695 ex.printStackTrace();
00696 }
00697 }
00698
00699 btnServo_.setImage(servoOffIcon_);
00700 btnServo_.setToolTipText("Servo Off");
00701 } catch (Exception e) {
00702 if(hwCtrl_ != null)
00703 hwCtrl_.power("all", SwitchStatus.SWITCH_OFF);
00704 GrxDebugUtil.printErr("got exception during servo on process:", e);
00705 }
00706 } else {
00707 if(hwCtrl_ != null)
00708 hwCtrl_.power("all", SwitchStatus.SWITCH_OFF);
00709 btnServo_.setSelection(false);
00710 }
00711 }
00712
00713 private void servoOff() {
00714 boolean ans = MessageDialog.openConfirm(getParent().getShell(),
00715 "!! Robot Motion Warning (SERVO OFF) !!\n\n" +
00716 "Push [OK] to servo OFF.\n",
00717 "Servo OFF");
00718 if (ans) {
00719 try {
00720 if(hwCtrl_ != null) {
00721 hwCtrl_.servo("all", SwitchStatus.SWITCH_OFF);
00722 hwCtrl_.power("all", SwitchStatus.SWITCH_OFF);
00723 }
00724 btnServo_.setImage(servoOnIcon_);
00725 btnServo_.setToolTipText("Servo On");
00726 } catch (Exception e) {
00727 GrxDebugUtil.printErr("got exception during servo off process:", e);
00728 }
00729 } else {
00730 btnServo_.setSelection(true);
00731 }
00732 }
00733
00734 public boolean isAnyServoOn() {
00735 final int SERVO_STATE_MASK = 0x2;
00736 if(hwCtrl_ != null) {
00737 hwCtrl_.getStatus(robotStateH_);
00738 int[][] state = robotStateH_.value.servoState;
00739 for (int i=0; i<currentModel_.getDOF(); i++) {
00740 if ((state[i][0]&SERVO_STATE_MASK) != 0) {
00741 return true;
00742 }
00743 }
00744 }
00745 return false;
00746 }
00747 }