GrxRobotHardwareClientView.java
Go to the documentation of this file.
1 package com.generalrobotix.ui.view;
2 
3 import java.util.ArrayList;
4 import java.util.Date;
5 
6 import jp.go.aist.hrp.simulator.DynamicsSimulator;
7 import jp.go.aist.hrp.simulator.DynamicsSimulatorFactory;
8 import jp.go.aist.hrp.simulator.DynamicsSimulatorFactoryHelper;
9 import jp.go.aist.hrp.simulator.SensorState;
10 import jp.go.aist.hrp.simulator.WorldStateHolder;
11 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.IntegrateMethod;
12 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.LinkDataType;
13 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.SensorOption;
14 
15 import org.eclipse.jface.dialogs.MessageDialog;
16 import org.eclipse.swt.SWT;
17 import org.eclipse.swt.events.KeyAdapter;
18 import org.eclipse.swt.events.KeyEvent;
19 import org.eclipse.swt.events.SelectionEvent;
20 import org.eclipse.swt.events.SelectionListener;
21 import org.eclipse.swt.graphics.Image;
22 import org.eclipse.swt.layout.GridData;
23 import org.eclipse.swt.layout.GridLayout;
24 import org.eclipse.swt.widgets.Button;
25 import org.eclipse.swt.widgets.Composite;
26 import org.eclipse.swt.widgets.Display;
27 import org.eclipse.swt.widgets.Label;
28 import org.eclipse.swt.widgets.Text;
29 
30 import OpenHRP.RobotHardwareService;
31 import OpenHRP.RobotHardwareServiceHelper;
32 import OpenHRP.StateHolderService;
33 import OpenHRP.StateHolderServiceHelper;
34 import OpenHRP.StateHolderServicePackage.CommandHolder;
35 import OpenHRP.RobotHardwareServicePackage.RobotStateHolder;
36 import OpenHRP.RobotHardwareServicePackage.SwitchStatus;
37 import RTC.ComponentProfile;
38 import RTC.ConnectorProfile;
39 import RTC.ConnectorProfileHolder;
40 import RTC.ExecutionContext;
41 import RTC.LifeCycleState;
42 import RTC.PortInterfaceProfile;
43 import RTC.PortProfile;
44 import RTC.PortService;
45 import RTC.RTObject;
46 import RTC.RTObjectHelper;
47 
48 import com.generalrobotix.ui.GrxBaseItem;
49 import com.generalrobotix.ui.GrxBaseView;
50 import com.generalrobotix.ui.GrxBaseViewPart;
51 import com.generalrobotix.ui.GrxPluginManager;
52 import com.generalrobotix.ui.grxui.Activator;
53 import com.generalrobotix.ui.item.GrxModelItem;
54 import com.generalrobotix.ui.item.GrxWorldStateItem;
55 import com.generalrobotix.ui.item.GrxWorldStateItem.WorldStateEx;
56 import com.generalrobotix.ui.util.GrxCorbaUtil;
57 import com.generalrobotix.ui.util.GrxDebugUtil;
58 import com.generalrobotix.ui.util.GrxXmlUtil;
59 
60 @SuppressWarnings("serial")
64 public class GrxRobotHardwareClientView extends GrxBaseView {
65  public static final String TITLE = "RobotHardware RTC Client";
66  private static final int NOT_CONNECTED = 0;
67  private static final int CONNECTING = 1;
68  private static final int CONNECTED = 2;
69 
70  private GrxJythonPromptView jythonView_;
71 
72  private StateHolderService sholder_;
73  private RobotHardwareService hwCtrl_;
74 
75  private DynamicsSimulator dynamics_;
76  private GrxWorldStateItem currentItem_;
77  private GrxModelItem currentModel_;
78 
79  private WorldStateHolder worldStateH_ = new WorldStateHolder();
80  private RobotStateHolder robotStateH_ = new RobotStateHolder();
81 
82  private String robotType_ = "-----";
83  private String robotHost_ = "localhost";
84  private int robotPort_ = 2809;
85  private int interval_ = 200;
86  private String setupFile_ = "-----";
87  private String RobotHardwareRTC_ = "RobotHardware0";
88  private String StateHolderRTC_ = "StateHolder0";
89 
90  private Date initialDate_;
91  private Date prevDate_;
92  private int state_ = NOT_CONNECTED;
93  private boolean actualFlag = false;
94  private Image startMonitorIcon_ = Activator.getDefault().getImage("sim_start.png");
95  private Image stopMonitorIcon_ = Activator.getDefault().getImage("sim_stop.png");
96  private Image servoOnIcon_ = Activator.getDefault().getImage("robot_servo_start.png");
97  private Image servoOffIcon_ = Activator.getDefault().getImage("robot_servo_stop.png");
98  private Image lampOnIcon_ = Activator.getDefault().getImage("lamp_on.png");
99  private Image lampOffIcon_ = Activator.getDefault().getImage("lamp_off.png");
100  private Button btnConnect_;
101  private Button btnServo_;
102  private Button btnSetup_;
103  private Label lblLamp_;
104  private Label lblStatus_;
105  private Text fldStatus_;
106  private Composite propertyPanel_;
107  private boolean isMonitorRunning_;
108 
109  private ArrayList<SetPropertyPanel> propList_ = new ArrayList<SetPropertyPanel>();
110 
118  public GrxRobotHardwareClientView(String name, GrxPluginManager manager, GrxBaseViewPart vp, Composite parent) {
119  super(name, manager, vp, parent);
120 
121  composite_.setLayout(new GridLayout(1,false));
122  Composite layout1 = new Composite(composite_, SWT.NONE);
123  layout1.setLayoutData(new GridData(GridData.HORIZONTAL_ALIGN_CENTER));
124  GridLayout gridLayout = new GridLayout(6, false);
125  gridLayout.marginWidth = 5;
126  gridLayout.horizontalSpacing = 5;
127  layout1.setLayout(gridLayout);
128  // servo on/off button
129  btnServo_ = new Button(layout1, SWT.TOGGLE);
130  btnServo_.setImage(servoOnIcon_);
131  GridData btnGridData = new GridData();
132  btnGridData.widthHint = 32;
133  btnGridData.heightHint = 32;
134  btnServo_.setLayoutData(btnGridData);
135  // status label
136  lblStatus_ = new Label(layout1, SWT.NONE);
137  lblStatus_.setText("Status");
138  // lamp label
139  lblLamp_ = new Label(layout1, SWT.NONE);
140  lblLamp_.setImage(lampOffIcon_);
141  // status filed
142  fldStatus_ = new Text(layout1, SWT.SINGLE | SWT.BORDER);
143  fldStatus_.setText("Not Connected.");
144  GridData textGridData = new GridData();
145  textGridData.widthHint = 100;
146  fldStatus_.setLayoutData(textGridData);
147  // connect button
148  btnConnect_ = new Button(layout1, SWT.TOGGLE);
149  btnConnect_.setLayoutData(btnGridData);
150  btnConnect_.setImage(startMonitorIcon_);
151  // setup button
152  btnSetup_ = new Button(layout1, SWT.NONE);
153  btnSetup_.setText("ROBOT SETUP");
154 
155  propertyPanel_ = new Composite(composite_,SWT.NONE);
156  propertyPanel_.setLayoutData(new GridData(GridData.HORIZONTAL_ALIGN_CENTER));
157  GridLayout gridLayout1 = new GridLayout(1, false);
158  gridLayout.marginWidth = 5;
159  gridLayout.horizontalSpacing = 5;
160  propertyPanel_.setLayout(gridLayout1);
161  propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "Robot Host", "robotHost", true, robotHost_));
162  propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "Robot Port", "robotPort", true, new Integer(robotPort_).toString()));
163  propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "Robot Name", "ROBOT", true, robotType_));
164  propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "Interval[ms]","interval", true, new Integer(interval_).toString()));
165  propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "Setup File", "setupFile", true, setupFile_));
166  propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "RobotHardwareService", "RobotHardwareServiceRTC", true, RobotHardwareRTC_));
167  propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "StateHolderService", "StateHolderRTC", true, StateHolderRTC_));
168 
169  btnConnect_.addSelectionListener(new SelectionListener() {
170  public void widgetDefaultSelected(SelectionEvent e) {}
171  public void widgetSelected(SelectionEvent e) {
172  if (btnConnect_.getSelection()) {
173  startMonitor(true);
174  } else {
175  boolean ans = MessageDialog.openConfirm(getParent().getShell(),
176  "Do you really want to stop monitoring?",
177  "Stopping Robot State Monitor");
178 
179  if (ans){
180  stopMonitor();
181  }else{
182  btnConnect_.setSelection(true);
183  }
184  }
185  }
186  });
187 
188  btnSetup_.setEnabled(false);
189  btnSetup_.addSelectionListener(new SelectionListener() {
190  public void widgetDefaultSelected(SelectionEvent e) {}
191  public void widgetSelected(SelectionEvent e) {
192  setupFile_ = getProperty("setupFile");
193  getJythonView().execFile(GrxXmlUtil.expandEnvVal(setupFile_));
194  }
195  });
196 
197  btnServo_.setEnabled(false);
198  btnServo_.addSelectionListener(new SelectionListener() {
199  public void widgetDefaultSelected(SelectionEvent e) {}
200  public void widgetSelected(SelectionEvent e) {
201  if (btnServo_.getSelection()){
202  servoOn();
203  }else{
204  servoOff();
205  }
206  }
207  });
208 
209  currentItem_ = manager_.<GrxWorldStateItem>getSelectedItem(GrxWorldStateItem.class, null);
210  if(currentItem_!=null){
211  currentItem_.addObserver(this);
212  }
213  manager_.registerItemChangeListener(this, GrxWorldStateItem.class);
214  }
215 
216  public void registerItemChange(GrxBaseItem item, int event){
217  if(item instanceof GrxWorldStateItem){
218  GrxWorldStateItem worldStateItem = (GrxWorldStateItem) item;
219  switch(event){
220  case GrxPluginManager.SELECTED_ITEM:
221  if(currentItem_!=worldStateItem){
222  currentItem_ = worldStateItem;
223  currentItem_.addObserver(this);
224  }
225  break;
226  case GrxPluginManager.REMOVE_ITEM:
227  case GrxPluginManager.NOTSELECTED_ITEM:
228  if(currentItem_==worldStateItem){
229  currentItem_.deleteObserver(this);
230  currentItem_ = null;
231  }
232  break;
233  default:
234  break;
235  }
236  }
237  }
238 
239  private class SetPropertyPanel extends Composite {
240  private String propName;
241  private boolean isLocal = true;
242  private String defaultVal;
243 
244  private Label label;
245  private Text fld;
246  private Button set;
247  private Button resume;
248 
249 
250  public SetPropertyPanel(Composite parent, int style, String _title, String _propName, boolean _isLocal, String _defaultVal) {
251  super(parent, style);
252  GridLayout gridLayout = new GridLayout(4, false);
253  gridLayout.marginWidth = 5;
254  gridLayout.horizontalSpacing = 5;
255  this.setLayout(gridLayout);
256  GridData labelGridData = new GridData();
257  labelGridData.widthHint = 150;
258  label = new Label(this, SWT.NONE);
259  label.setText(_title);
260  label.setLayoutData(labelGridData);
261  fld = new Text(this, SWT.NONE);
262  GridData textGridData = new GridData();
263  textGridData.widthHint = 120;
264  fld.setLayoutData(textGridData);
265  set = new Button(this, SWT.NONE);
266  set.setText("Set");
267  resume = new Button(this, SWT.NONE);
268  resume.setText("Resume");
269  propName = _propName;
270  isLocal = _isLocal;
271  defaultVal = _defaultVal;
272 
273  fld.addKeyListener(new KeyAdapter() {
274  public void keyReleased(KeyEvent e){
275  if (e.keyCode == SWT.CR) {
276  set();
277  } else {
278  boolean hasChanged = !fld.getText().equals(getValue());
279  set.setEnabled(hasChanged);
280  resume.setEnabled(hasChanged);
281  }
282  }
283  });
284 
285  set.setEnabled(false);
286  set.addSelectionListener(new SelectionListener() {
287  public void widgetDefaultSelected(SelectionEvent e) {
288  }
289  public void widgetSelected(SelectionEvent e) {
290  set();
291  }
292  });
293 
294  resume.setEnabled(false);
295  resume.addSelectionListener(new SelectionListener() {
296  public void widgetDefaultSelected(SelectionEvent e) {
297  }
298  public void widgetSelected(SelectionEvent e) {
299  resume();
300  }
301  });
302  }
303 
304  public void setEditable(boolean isEditable) {
305  fld.setEditable(isEditable);
306  resume.setEnabled(isEditable);
307  set.setEnabled(isEditable);
308  }
309 
310  public String getValue() {
311  String str;
312  if (isLocal)
313  str = getProperty(propName);
314  else
315  str = manager_.getProjectProperty(propName);
316  if (str == null)
317  str = defaultVal;
318  return str;
319  }
320 
321  public void set() {
322  String value = fld.getText();
323  if (isLocal){
324  setProperty(propName, value);
325  }else{
326  manager_.setProjectProperty(propName, value);
327  System.out.println("("+propName+","+value+")");
328  }
329  set.setEnabled(false);
330  resume.setEnabled(false);
331  }
332 
333  public void resume() {
334  fld.setText(getValue());
335  set.setEnabled(false);
336  resume.setEnabled(false);
337  }
338  }
339 
340  private void startMonitor(final boolean isInteractive) {
341  if (isMonitorRunning_) return;
342 
343  isMonitorRunning_ = true;
344  btnConnect_.setImage(stopMonitorIcon_);
345  btnConnect_.setToolTipText("Stop Robot State Monitor");
346  btnConnect_.setSelection(true);
347 
348  setConnectionState(CONNECTING);
349 
350  //GrxGuiUtil.setEnableRecursive(false, propertyPanel_, null);
351 
352  Runnable runnable = new Runnable() {
353  public void run(){
354  Display display = composite_.getDisplay();
355  if (state_ == CONNECTING){
356  tryConnection(isInteractive);
357  if (!display.isDisposed())
358  display.timerExec(1000, this);
359  }else if (state_ == CONNECTED){
360  try{
361  updateRobotState();
362  }catch(Exception ex){
363  ex.printStackTrace();
364  }
365  interval_ = getInt("interval", 200);
366  if (!display.isDisposed())
367  display.timerExec(interval_, this);
368  }else{
369  }
370  }
371  };
372  Display display = composite_.getDisplay();
373  if (!display.isDisposed()){
374  display.timerExec(1, runnable);
375  }
376 
377 
378  }
379 
380  private void tryConnection(boolean isInteractive){
381  System.out.println("tryConnection()");
382  if (currentItem_ == null) return;
383  try {
384  for (int i=0; i<propList_.size(); i++)
385  propList_.get(i).resume();
386 
387  // set property for connection
388  robotHost_ = getStr("robotHost");
389  if (robotHost_ != null){
390  manager_.setProjectProperty("nsHost", robotHost_);
391  }
392  try {
393  robotPort_ = Integer.parseInt(manager_.getProjectProperty("robotPort"));
394  } catch (Exception e) {
395  robotPort_ = 2809; // if not set try the default port number
396  }
397 
398  robotType_ = getStr("ROBOT");
399 
400  currentModel_ = (GrxModelItem) manager_.getItem(GrxModelItem.class, robotType_);
401  if (currentModel_ == null){
402  System.out.println("Can't find robot model("+robotType_+")");
403  return;
404  }
405 
406  //actualFlag = isTrue("showActualState", false);
407 
408  // initialize logger
409  currentItem_.clearLog();
410  currentItem_.registerCharacter(robotType_, currentModel_.getBodyInfo());
411 
412  // get RobotHardwareService
413  System.out.println("RobotHardwareRTC_ = "+RobotHardwareRTC_);
414  RTObject rtc = findRTC(RobotHardwareRTC_);
415  ExecutionContext ec = rtc.get_owned_contexts()[0];
416  if (ec.get_component_state(rtc) != LifeCycleState.ACTIVE_STATE){
417  ec.activate_component(rtc);
418  }
419  hwCtrl_ = RobotHardwareServiceHelper.narrow(findService(rtc, "service0"));
420 
421  // initialize servo On button
422  if (isAnyServoOn()) {
423  btnServo_.setSelection(true);
424  btnServo_.setImage(servoOffIcon_);
425  btnServo_.setToolTipText("Servo Off");
426  } else {
427  btnServo_.setSelection(false);
428  btnServo_.setImage(servoOnIcon_);
429  btnServo_.setToolTipText("Servo On");
430  }
431  prevDate_ = new Date();
432 
433  initDynamicsSimulator(false);
434 
435  setConnectionState(CONNECTED);
436 
437  } catch (Exception e) {
438  //GrxDebugUtil.printErr("", e);
439 
440  if (isInteractive && currentModel_ == null) {
441  MessageDialog.openInformation(getParent().getShell(),
442  "Load Model(" + robotType_ + ") first.",
443  "Start Robot State Monitor");
444  stopMonitor();
445  } else {
446  setConnectionState(CONNECTING);
447  }
448  }
449  }
450 
451  private void stopMonitor() {
452  btnConnect_.setImage(startMonitorIcon_);
453  btnConnect_.setToolTipText("Start Robot State Monitor");
454  btnConnect_.setSelection(false);
455 
456  setConnectionState(NOT_CONNECTED);
457  isMonitorRunning_ = false;
458  }
459 
460  private void setConnectionState(int state) {
461  switch (state) {
462  case NOT_CONNECTED:
463  btnSetup_.setEnabled(false);
464  btnServo_.setEnabled(false);
465  fldStatus_.setText("Not Connected.");
466  lblLamp_.setImage(lampOffIcon_);
467  //GrxGuiUtil.setEnableRecursive(true, propertyPanel_, null);
468  for (int i=0; i<propList_.size(); i++) {
469  propList_.get(i).set.setEnabled(false);
470  propList_.get(i).resume.setEnabled(false);
471  }
472 
473  break;
474 
475  case CONNECTING:
476  btnSetup_.setEnabled(false);
477  btnServo_.setEnabled(false);
478  fldStatus_.setText("Connecting ...");
479  //GrxGuiUtil.setEnableRecursive(false, propertyPanel_, null);
480  break;
481 
482  case CONNECTED:
483  btnSetup_.setEnabled(true);
484  btnServo_.setEnabled(true);
485  fldStatus_.setText("Connected.");
486  lblLamp_.setImage(lampOnIcon_);
487  break;
488  }
489  state_ = state;
490  }
491 
492  public void restoreProperties() {
493  super.restoreProperties();
494  robotHost_ = getStr("robotHost");
495  if (robotHost_ != null){
496  manager_.setProjectProperty("nsHost", robotHost_);
497 
498  }
499  startMonitor(false);
500  }
501 
502  public RTObject findRTC(String name){
503  org.omg.CORBA.Object obj = GrxCorbaUtil.getReference(name, "rtc", robotHost_, robotPort_);
504  return RTObjectHelper.narrow(obj);
505  }
506 
507  public org.omg.CORBA.Object findService(RTObject rtc, String service){
508  ComponentProfile prof = rtc.get_component_profile();
509  PortProfile[] port_prof = prof.port_profiles;
510  PortService port = null;
511  for (int i=0; i<port_prof.length; i++){
512  PortProfile pp = port_prof[i];
513  PortInterfaceProfile[] ifs = pp.interfaces;
514  for (int j=0; j<ifs.length; j++){
515  PortInterfaceProfile aif = ifs[j];
516  if (aif.instance_name.equals(service)){
517  port = pp.port_ref;
518  }
519  }
520  }
521  ConnectorProfile con_prof = new ConnectorProfile();
522  con_prof.name = "noname";
523  con_prof.connector_id = "";
524  con_prof.ports = new PortService[1];
525  con_prof.ports[0] = port;
526  con_prof.properties = new _SDOPackage.NameValue[0];
527  ConnectorProfileHolder con_prof_holder = new ConnectorProfileHolder();
528  con_prof_holder.value = con_prof;
529  port.connect(con_prof_holder);
530  String ior = con_prof_holder.value.properties[0].value.extract_string();
531  port.disconnect(con_prof_holder.value.connector_id);
532  return GrxCorbaUtil.getORB().string_to_object(ior);
533  }
534 
535  // get the data here and stuff it into currentItem_
536  private void updateRobotState(){
537  //if (currentItem_ == null || !btnConnect_.isSelected()) {
538  if (currentItem_ == null) {
539  stopMonitor();
540  return;
541  }
542 
543  Date now = new Date();
544  long time = now.getTime();
545  if (prevDate_ != null) {
546  time -= prevDate_.getTime();
547  }
548  if (state_ == CONNECTING) {
549  if (time > 1000) {
550  prevDate_ = now;
551  if (lblLamp_.getImage() == lampOffIcon_)
552  lblLamp_.setImage(lampOnIcon_);
553  else
554  lblLamp_.setImage(lampOffIcon_);
555  startMonitor(false);
556  }
557  return;
558  }
559 
560  if (state_ == CONNECTED && time > interval_) {
561  prevDate_ = now;
562  try {
563  if (hwCtrl_ != null){
564  hwCtrl_.getStatus(robotStateH_);
565  }
566 
567  if(actualFlag) {
568  dynamics_.setCharacterAllLinkData(robotType_, LinkDataType.JOINT_VALUE, robotStateH_.value.angle);
569  } else {
570  //dynamics_.setCharacterAllLinkData(robotType_, LinkDataType.JOINT_VALUE, robotStateH_.value.command);
571  }
572 
573  try{
574  if (sholder_ == null){
575  StateHolderRTC_ = getStr("StateHolderRTC", StateHolderRTC_);
576  RTObject rtc = findRTC(StateHolderRTC_);
577  sholder_ = StateHolderServiceHelper.narrow(findService(rtc, "service0"));
578  }
579  CommandHolder com = new CommandHolder();
580  sholder_.getCommand(com);
581  dynamics_.setCharacterLinkData(robotType_, currentModel_.rootLink().getName(), LinkDataType.ABS_TRANSFORM, com.value.baseTransform);
582  dynamics_.setCharacterAllLinkData(robotType_, LinkDataType.JOINT_VALUE, robotStateH_.value.command);
583  }catch(Exception e){
584  GrxDebugUtil.printErr("failed to connect StateHolderService");
585  sholder_ = null;
586  }
587 
588  dynamics_.calcWorldForwardKinematics();
589  dynamics_.getWorldState(worldStateH_);
590 
591  } catch (Exception e) {
592  GrxDebugUtil.printErr("iobclient got exception:", e);
593  setConnectionState(CONNECTING);
594  return;
595  }
596 
597  SensorState ss = new SensorState();
598  ss.q = robotStateH_.value.angle;
599  ss.u = robotStateH_.value.torque;
600  ss.force = robotStateH_.value.force;
601  ss.accel = robotStateH_.value.accel;
602  ss.rateGyro = robotStateH_.value.rateGyro;
603 
604  WorldStateEx wsx = new WorldStateEx(worldStateH_.value);
605  if (currentItem_.getLogSize() == 0){
606  initialDate_ = now;
607  }
608  wsx.time = (now.getTime() - initialDate_.getTime()) / 1000.0;
609  wsx.collisions = null;
610  wsx.setSensorState(robotType_, ss);
611  wsx.setTargetState(robotType_, robotStateH_.value.command);
612  int [] sstate = new int[robotStateH_.value.servoState.length];
613  for (int i=0; i<sstate.length; i++){
614  sstate[i] = robotStateH_.value.servoState[i][0];
615  }
616  wsx.setServoState(robotType_, sstate);
617  wsx.setPowerState(robotType_, robotStateH_.value.voltage, robotStateH_.value.current);
618  currentItem_.addValue(wsx.time, wsx);
619  currentItem_.setPosition(currentItem_.getLogSize()-1);
620  }
621  }
622 
623  private DynamicsSimulator initDynamicsSimulator(boolean update) {
624  if (dynamics_ != null && !update) {
625  return dynamics_;
626  }
627  org.omg.CORBA.Object obj = GrxCorbaUtil.getReference("DynamicsSimulatorFactory");
628  DynamicsSimulatorFactory dynFactory = DynamicsSimulatorFactoryHelper.narrow(obj);
629  if (dynamics_ != null) {
630  try {
631  dynamics_.destroy();
632  } catch(Exception e) {
633  GrxDebugUtil.printErr("", e);
634  }
635  }
636  try {
637  dynamics_ = dynFactory.create();
638  dynamics_.registerCharacter(robotType_, currentModel_.getBodyInfo());
639  dynamics_.init(0.001, IntegrateMethod.EULER, SensorOption.DISABLE_SENSOR);
640  } catch (Exception e) {
641  dynamics_ = null;
642  e.printStackTrace();
643  }
644 
645  return dynamics_;
646  }
647 
648  private GrxJythonPromptView getJythonView() {
649  if (jythonView_ == null) {
650  jythonView_= (GrxJythonPromptView)manager_.getView(GrxJythonPromptView.class,true);
651  }
652 
653  return jythonView_;
654  }
655 
656  private void servoOn() {
657  if (isAnyServoOn()) {
658  return;
659  }
660  if(hwCtrl_ != null) {
661  hwCtrl_.power("all", SwitchStatus.SWITCH_ON);
662  try{
663  Thread.sleep(1000);
664  }catch(Exception ex){
665  ex.printStackTrace();
666  }
667  }
668 
669  boolean ans = MessageDialog.openConfirm(getParent().getShell(),
670  "!! Robot Motion Warning (SERVO ON) !!\n" +
671  "Confirm RELAY turned ON.\n" +
672  "Then Push [OK] to servo ON.\n",
673  "Servo ON");
674  if (ans) {
675  try {
676  // get StateHolderService
677  StateHolderRTC_ = getStr("StateHolderRTC", StateHolderRTC_);
678  RTObject rtc = findRTC(StateHolderRTC_);
679  sholder_ = StateHolderServiceHelper.narrow(findService(rtc, "service0"));
680 
681  if (sholder_ != null) {
682  sholder_.goActual();
683  try{
684  Thread.sleep(100);
685  }catch(Exception ex){
686  ex.printStackTrace();
687  }
688  }
689 
690  if(hwCtrl_ != null){
691  hwCtrl_.servo("all", SwitchStatus.SWITCH_ON);
692  try {
693  Thread.sleep(5000);
694  }catch(Exception ex){
695  ex.printStackTrace();
696  }
697  }
698 
699  btnServo_.setImage(servoOffIcon_);
700  btnServo_.setToolTipText("Servo Off");
701  } catch (Exception e) {
702  if(hwCtrl_ != null)
703  hwCtrl_.power("all", SwitchStatus.SWITCH_OFF);
704  GrxDebugUtil.printErr("got exception during servo on process:", e);
705  }
706  } else {
707  if(hwCtrl_ != null)
708  hwCtrl_.power("all", SwitchStatus.SWITCH_OFF);
709  btnServo_.setSelection(false);
710  }
711  }
712 
713  private void servoOff() {
714  boolean ans = MessageDialog.openConfirm(getParent().getShell(),
715  "!! Robot Motion Warning (SERVO OFF) !!\n\n" +
716  "Push [OK] to servo OFF.\n",
717  "Servo OFF");
718  if (ans) {
719  try {
720  if(hwCtrl_ != null) {
721  hwCtrl_.servo("all", SwitchStatus.SWITCH_OFF);
722  hwCtrl_.power("all", SwitchStatus.SWITCH_OFF);
723  }
724  btnServo_.setImage(servoOnIcon_);
725  btnServo_.setToolTipText("Servo On");
726  } catch (Exception e) {
727  GrxDebugUtil.printErr("got exception during servo off process:", e);
728  }
729  } else {
730  btnServo_.setSelection(true);
731  }
732  }
733 
734  public boolean isAnyServoOn() {
735  final int SERVO_STATE_MASK = 0x2;
736  if(hwCtrl_ != null) {
737  hwCtrl_.getStatus(robotStateH_);
738  int[][] state = robotStateH_.value.servoState;
739  for (int i=0; i<currentModel_.getDOF(); i++) {
740  if ((state[i][0]&SERVO_STATE_MASK) != 0) {
741  return true;
742  }
743  }
744  }
745  return false;
746  }
747 }
com::generalrobotix::ui.view.GrxRobotHardwareClientView.GrxRobotHardwareClientView
GrxRobotHardwareClientView(String name, GrxPluginManager manager, GrxBaseViewPart vp, Composite parent)
constructor
Definition: GrxRobotHardwareClientView.java:118
com::generalrobotix::ui::item::GrxWorldStateItem::WorldStateEx::setSensorState
void setSensorState(String charName, SensorState state)
com::generalrobotix::ui::item::GrxWorldStateItem::registerCharacter
void registerCharacter(String cname, BodyInfo binfo)
toString
bool toString(IOP::IOR &ior, std::string &iorstr)
ior
ior
state
state
null
#define null
_SDOPackage
rtm.findRTC
def findRTC(name, rnc=None)
get RT component
Definition: jython/rtm.py:341
value
png_voidp int value
PA10.servoOn
def servoOn(joint="all")
Definition: PA10.py:77
ifs
ifs
com::generalrobotix::ui::item::GrxWorldStateItem::WorldStateEx::setTargetState
void setTargetState(String charName, double[] targets)
com::generalrobotix::ui.view.GrxRobotHardwareClientView.SetPropertyPanel.SetPropertyPanel
SetPropertyPanel(Composite parent, int style, String _title, String _propName, boolean _isLocal, String _defaultVal)
Definition: GrxRobotHardwareClientView.java:250
i
png_uint_32 i
com::generalrobotix::ui::util::GrxCorbaUtil::getORB
static org.omg.CORBA.ORB getORB(String[] argv)
rtm.findService
def findService(rtc, port_name, type_name, instance_name)
get a service of RT component
Definition: jython/rtm.py:641
j
def j(str, encoding="cp932")
PA10.servoOff
def servoOff(joint="all")
Definition: PA10.py:86
run
def run(tree, args)
com::generalrobotix::ui::util::GrxXmlUtil::expandEnvVal
static String expandEnvVal(String str)
com::generalrobotix::ui::util::GrxCorbaUtil::getReference
static org.omg.CORBA.Object getReference(String id)
LifeCycleState
LifeCycleState
com::generalrobotix::ui::item::GrxWorldStateItem::WorldStateEx::setServoState
void setServoState(String charName, int[] servoStat)
com::generalrobotix::ui.view.GrxRobotHardwareClientView.findService
org.omg.CORBA.Object findService(RTObject rtc, String service)
Definition: GrxRobotHardwareClientView.java:507
org
org
obj
obj
ec
ec
com::generalrobotix::ui::item::GrxWorldStateItem::WorldStateEx::setPowerState
void setPowerState(String charName, double voltage, double current)

hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:50