BehaviorManager.java
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * General Robotix Inc.
8  * National Institute of Advanced Industrial Science and Technology (AIST)
9  */
17 package com.generalrobotix.ui.view.tdview;
18 
19 import java.util.Iterator;
20 import java.util.List;
21 import java.util.Map;
22 import java.util.Vector;
23 
24 import javax.media.j3d.BranchGroup;
25 import javax.media.j3d.Transform3D;
26 import javax.media.j3d.TransformGroup;
27 
28 import org.eclipse.jface.dialogs.MessageDialog;
29 import org.eclipse.swt.widgets.Display;
30 
31 import jp.go.aist.hrp.simulator.BodyInfo;
32 import jp.go.aist.hrp.simulator.DynamicsSimulator;
33 import jp.go.aist.hrp.simulator.DynamicsSimulatorFactory;
34 import jp.go.aist.hrp.simulator.DynamicsSimulatorFactoryHelper;
35 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.IntegrateMethod;
36 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.LinkDataType;
37 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.SensorOption;
38 import jp.go.aist.hrp.simulator.*;
39 
50 import com.sun.j3d.utils.picking.PickCanvas;
51 import com.sun.j3d.utils.picking.PickTool;
52 
58 public class BehaviorManager implements WorldReplaceListener {
59  //--------------------------------------------------------------------
60  // 定数
61  // オペレーションモード
62  public static final int OPERATION_MODE_NONE = 0;
63  public static final int OBJECT_ROTATION_MODE = 1;
64  public static final int OBJECT_TRANSLATION_MODE = 2;
65  public static final int JOINT_ROTATION_MODE = 3;
66  public static final int FITTING_FROM_MODE = 4;
67  public static final int FITTING_TO_MODE = 5;
68  public static final int INV_KINEMA_FROM_MODE = 6;
69  public static final int INV_KINEMA_TRANSLATION_MODE = 7;
70  public static final int INV_KINEMA_ROTATION_MODE = 8;
71 
72  // ビューモード
73  public static final int WALK_VIEW_MODE = 1;
74  public static final int ROOM_VIEW_MODE = 2;
75  public static final int PARALLEL_VIEW_MODE = 3;
76 
77  // インスタンス変数
78  DynamicsSimulator currentDynamics_;
79  private IseBehavior behavior_;
80  private IseBehaviorHandler handler_;
81  private int viewMode_;
82  private int operationMode_;
84  private BehaviorInfo info_;
87  private Grx3DView viewer_;
89  private boolean itemChangeFlag_ = false;
90  private boolean messageSkip_ = false;
91 
92  private List<GrxModelItem> currentModels_ = null;
93  private List<GrxCollisionPairItem> currentCollisionPairs_ = null;
94 
95  //--------------------------------------------------------------------
96  // コンストラクタ
98  manager_ = manager;
99  resolver_ = new InvKinemaResolver(manager_);
100  //Simulator.getInstance().addWorldReplaceListener(this);
101  }
102 
103  // 公開メソッド
104  public void setThreeDViewer(Grx3DView viewer) {
105  viewer_ = viewer;
106  PickCanvas pickCanvas = new PickCanvas(
107  viewer.getCanvas3D(),
108  viewer.getBranchGroupRoot()
109  );
110  pickCanvas.setMode(PickTool.GEOMETRY_INTERSECT_INFO);
111 
112  handler_ = new IseBehaviorHandler(this);
113 
114  behavior_ = new IseBehavior(handler_);
115 
116  BranchGroup bg = new BranchGroup();
117  bg.addChild(behavior_);
118 
119  BranchGroup bgRoot = viewer_.getBranchGroupRoot();
120  bgRoot.addChild(bg);
121 
122  //drawable_ = viewer.getDrawable();
123  drawable_ = viewer;
124 
125  ViewInfo viewInfo = drawable_.getViewInfo();
126  switch (viewInfo.getViewMode()) {
128  setViewMode(ROOM_VIEW_MODE);
129  break;
131  setViewMode(WALK_VIEW_MODE);
132  break;
134  setViewMode(PARALLEL_VIEW_MODE);
135  break;
136  }
137  //TransformGroup tgView = drawable_.getTransformGroupRoot();
138  info_ = new BehaviorInfo(manager_, pickCanvas, drawable_);
139 
140  behavior_.setBehaviorInfo(info_);
141  }
142 
143  public void setViewIndicator(BehaviorHandler handler) {
144  indicator_ = handler;
145  }
146 
147  public void setViewMode(int mode) {
148  viewMode_ = mode;
149  handler_.setViewMode(viewMode_);
150  }
151 
152  public void setOperationMode(int mode) {
153  operationMode_ = mode;
154  handler_.setOperationMode(operationMode_);
155  }
156 
157  public void setViewHandlerMode(String str) {
158  handler_.setViewHandlerMode(str);
159  }
160 
161  public boolean fit() {
162  return handler_.fit(info_);
163  }
164 
165  public void setPickTarget(TransformGroup tg) {
166  BehaviorInfo info = behavior_.getBehaviorInfo();
167  handler_.setPickTarget(tg, info);
168  }
169 
170  //--------------------------------------------------------------------
171  // ViewChangeListenerの実装
172  public void viewChanged(ViewChangeEvent evt) {
173  Transform3D transform = new Transform3D();
174  evt.getTransformGroup().getTransform(transform);
175 
176  // データの通知は ViewNode に任せる
177  drawable_.setTransform(transform);
178  }
179 
180  //--------------------------------------------------------------------
181  // WorldReplaceListenerの実装
182  public void replaceWorld(List<GrxBaseItem> list) {
183  //handler_ = new IseBehaviorHandler();
184  //behavior_ = new IseBehavior(handler_);
185 
186  //BranchGroup bg = new BranchGroup();
187  //bg.addChild(behavior_);
188 
189  //BranchGroup bgRoot = viewer_.getBranchGroupRoot();
190  //bgRoot.addChild(bg);
191 
192  //InvKinemaResolver resolver = new InvKinemaResolver(world_);
193  //ProjectManager project = ProjectManager.getInstance();
194  //resolver.setDynamicsSimulator(project.getDynamicsSimulator());
195  //handler_.setInvKinemaResolver(resolver);
196 
197  if (handler_ != null)
198  handler_.setViewIndicator(indicator_);
199  }
200 
206  public DynamicsSimulator getDynamicsSimulator(boolean update) {
207  //currentDynamics_ = dynamicsMap_.get(currentWorld_);
208  if (update && currentDynamics_ != null) {
209  try {
210  currentDynamics_.destroy();
211  } catch (Exception e) {
212  GrxDebugUtil.printErr("getDynamicsSimulator: destroy failed."); //$NON-NLS-1$
213  }
214  currentDynamics_ = null;
215  }
216 
217  if (currentDynamics_ == null) {
218  try {
219  org.omg.CORBA.Object obj = //process_.get(DynamicsSimulatorID_).getReference();
220  GrxCorbaUtil.getReference("DynamicsSimulatorFactory"); //$NON-NLS-1$
221  DynamicsSimulatorFactory ifactory = DynamicsSimulatorFactoryHelper
222  .narrow(obj);
223  currentDynamics_ = ifactory.create();
224  currentDynamics_._non_existent();
225  //dynamicsMap_.put(currentWorld_, currentDynamics_);
226 
227  } catch (Exception e) {
228  GrxDebugUtil.printErr("getDynamicsSimulator: create failed."); //$NON-NLS-1$
229  currentDynamics_ = null;
230  }
231  }
232  return currentDynamics_;
233  }
234 
235  public int getOperationMode() {
236  return operationMode_;
237  }
238 
246  public boolean initDynamicsSimulator() {
247  boolean ModelModified = false;
248  Iterator<GrxModelItem> itr = currentModels_.iterator();
249  while(itr.hasNext()){
250  GrxModelItem model = itr.next();
251  if(model.isModified()){
252  ModelModified = true;
253  if(!messageSkip_){
254  final String name = model.getName();
255  Display display = Display.getDefault();
256  display.syncExec(new Runnable(){
257  public void run(){
258  MessageDialog.openInformation(null, "", MessageBundle.get("BehaviorManager.dialog.message.reloadModel")+name+") "); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
259  }
260  });
261  }
262  }
263  if(ModelModified) return false;
264  }
265  if(!itemChangeFlag_) return true;
266  if(getDynamicsSimulator(true) == null) return false;
267 
268  try {
269  // register characters
270  for (int i=0; i<currentModels_.size(); i++) {
271  final GrxModelItem model = currentModels_.get(i);
272  BodyInfo bodyInfo = model.getBodyInfo();
273  if(bodyInfo==null) return false;
274  currentDynamics_.registerCharacter(model.getName(), bodyInfo);
275  }
276 
277  IntegrateMethod m = IntegrateMethod.EULER;
278  currentDynamics_.init(0.005, m, SensorOption.ENABLE_SENSOR);
279  currentDynamics_.setGVector(new double[] { 0.0, 0.0, 9.8});
280 
281  // set position/orientation and joint angles
282  for (int i=0; i<currentModels_.size(); i++) {
283  GrxModelItem model = currentModels_.get(i);
284  if (model.links_ == null)
285  continue;
286 
287  GrxLinkItem base = model.rootLink();
288  currentDynamics_.setCharacterLinkData(
289  model.getName(), base.getName(), LinkDataType.ABS_TRANSFORM,
290  model.getTransformArray(base));
291 
292  currentDynamics_.setCharacterAllLinkData(
293  model.getName(), LinkDataType.JOINT_VALUE,
294  model.getJointValues());
295  }
296 
297  // set collision check pairs
298  Map<String, GrxModelItem> modelmap = (Map<String, GrxModelItem>)manager_.getItemMap(GrxModelItem.class);
299  for (int i=0; i<currentCollisionPairs_.size(); i++) {
300  GrxCollisionPairItem item = (GrxCollisionPairItem) currentCollisionPairs_.get(i);
301  GrxModelItem m1 = modelmap.get(item.getStr("objectName1", "")); //$NON-NLS-1$ //$NON-NLS-2$
302  GrxModelItem m2 = modelmap.get(item.getStr("objectName2", "")); //$NON-NLS-1$ //$NON-NLS-2$
303  if (m1 == null || m2 == null) continue;
304  Vector<GrxLinkItem> links1, links2;
305  String lname1 = item.getStr("jointName1",""); //$NON-NLS-1$ //$NON-NLS-2$
306  if (lname1.equals("")){ //$NON-NLS-1$
307  links1 = m1.links_;
308  }else{
309  links1 = new Vector<GrxLinkItem>();
310  GrxLinkItem l = m1.getLink(lname1);
311  if (l != null) links1.add(l);
312  }
313  String lname2 = item.getStr("jointName2",""); //$NON-NLS-1$ //$NON-NLS-2$
314  if (lname2.equals("")){ //$NON-NLS-1$
315  links2 = m2.links_;
316  }else{
317  links2 = new Vector<GrxLinkItem>();
318  GrxLinkItem l = m2.getLink(lname2);
319  if (l != null) links2.add(l);
320  }
321  for (int j=0; j<links1.size(); j++){
322  for (int k=0; k<links2.size(); k++){
323  currentDynamics_.registerIntersectionCheckPair(
324  m1.getName(), links1.get(j).getName(),
325  m2.getName(), links2.get(k).getName(),
326  links1.get(j).getDbl("tolerance",0.0)+links2.get(k).getDbl("tolerance",0.0)); //$NON-NLS-1$ //$NON-NLS-2$
327  }
328  }
329  }
330  //state_.value = null;
331  } catch (Exception e) {
332  GrxDebugUtil.printErr("initDynamicsSimulator:", e); //$NON-NLS-1$
333  return false;
334  }
335  resolver_.setDynamicsSimulator(currentDynamics_);
336  handler_.setInvKinemaResolver(resolver_);
337  itemChangeFlag_ = false;
338  return true;
339  }
340 
341  public void addClickListener( Grx3DViewClickListener listener ){
342  behavior_.addClickListener( listener );
343  }
345  behavior_.removeClickListener( listener );
346  }
347 
348  private boolean setCharacterData(){
349  if(currentCollisionPairs_.isEmpty() || currentModels_.isEmpty()) return false;
350  if(!initDynamicsSimulator()) return false;
351  if (currentDynamics_ == null) return false;
352  for (int i=0; i<currentModels_.size(); i++) {
353  GrxModelItem model = currentModels_.get(i);
354  String name = model.getName();
355  GrxLinkItem base = model.rootLink();
356  double[] data = model.getTransformArray(base);
357  currentDynamics_.setCharacterLinkData(name, base.getName(), LinkDataType.ABS_TRANSFORM, data);
358  data = model.getJointValues();
359  currentDynamics_.setCharacterAllLinkData(name, LinkDataType.JOINT_VALUE, data);
360  }
361  return true;
362  }
363 
371  public Collision[] getCollision() {
372  if(!setCharacterData())
373  return null;
374  if (currentDynamics_.checkCollision(true)){
375  WorldStateHolder wsH = new WorldStateHolder();
376  currentDynamics_.getWorldState(wsH);
377  return wsH.value.collisions;
378  }else{
379  return null;
380  }
381  }
382 
390  public Distance[] getDistance() {
391  if(!setCharacterData())
392  return null;
393  return currentDynamics_.checkDistance();
394  }
395 
403  public LinkPair[] getIntersection() {
404  if(!setCharacterData())
405  return null;
406  return currentDynamics_.checkIntersection(true);
407  }
408 
409  public void setItem(List<GrxModelItem> models, List<GrxCollisionPairItem> cols){
410  currentModels_ = models;
411  currentCollisionPairs_ = cols;
412  itemChangeFlag_ = true;
413  }
414 
416  try {
417  currentDynamics_.destroy();
418  } catch (Exception e) {
419  GrxDebugUtil.printErr("getDynamicsSimulator: destroy failed."); //$NON-NLS-1$
420  }
421  currentDynamics_ = null;
422  }
423 
424  public void setMessageSkip(boolean flg){
425  messageSkip_ = flg;
426  }
427 
428  public void setItemChange(){
429  itemChangeFlag_ = true;
430  }
431 }
static final String get(String key)
Distance [] getDistance()
get distance information
#define null
our own NULL pointer
Definition: IceTypes.h:57
GrxLinkItem getLink(String name)
get link from name
png_infop png_charpp name
Definition: png.h:2382
item corresponds to a robot model
png_uint_32 i
Definition: png.h:2735
double [] getTransformArray(GrxLinkItem link)
get transform of link in array form
LinkPair [] getIntersection()
get intersection information
GrxLinkItem rootLink()
get root link
boolean initDynamicsSimulator()
initialize dynamics server
Map<?, ?> getItemMap(Class<? extends GrxBaseItem > cls)
def run(tree, args)
void removeClickListener(Grx3DViewClickListener listener)
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
backing_store_ptr info
Definition: jmemsys.h:181
プラグイン管理クラス GrxUIの核になるクラス。プラグインのロード等の、初期化を実行する。 プラグインとそ...
DynamicsSimulator getDynamicsSimulator(boolean update)
create dynamics server
void setItem(List< GrxModelItem > models, List< GrxCollisionPairItem > cols)
org
JSAMPIMAGE data
Definition: jpeglib.h:945
Collision [] getCollision()
get collision information
void addClickListener(Grx3DViewClickListener listener)
void setDynamicsSimulator(DynamicsSimulator integrator)


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:02