GrxLinkItem.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  */
10 
11 /*
12  * GrxModelItem.java
13  *
14  * @author Yuichiro Kawasumi (General Robotix, Inc.)
15  * @author Shin'ichiro Nakaoka (AIST)
16  */
17 
18 package com.generalrobotix.ui.item;
19 
20 import java.io.File;
21 import java.util.Hashtable;
22 import java.util.Iterator;
23 import java.util.Map;
24 
25 import javax.media.j3d.Appearance;import javax.media.j3d.BadTransformException;
26 import javax.media.j3d.BranchGroup;
27 import javax.media.j3d.Geometry;
28 import javax.media.j3d.GeometryArray;
29 import javax.media.j3d.LineArray;
30 import javax.media.j3d.PolygonAttributes;
31 import javax.media.j3d.Shape3D;
32 import javax.media.j3d.Switch;
33 import javax.media.j3d.Transform3D;
34 import javax.media.j3d.TransformGroup;
35 import javax.vecmath.Color3f;
36 import javax.vecmath.Matrix3d;
37 import javax.vecmath.Point3f;
38 import javax.vecmath.Vector3d;
39 
40 import jp.go.aist.hrp.simulator.HwcInfo;
41 import jp.go.aist.hrp.simulator.LinkInfo;
42 import jp.go.aist.hrp.simulator.SensorInfo;
43 import jp.go.aist.hrp.simulator.ShapeInfo;
44 import jp.go.aist.hrp.simulator.SegmentInfo;
45 
46 import org.eclipse.jface.action.Action;
47 import org.eclipse.jface.dialogs.InputDialog;
48 import org.eclipse.jface.dialogs.MessageDialog;
49 import org.eclipse.osgi.util.NLS;
50 import org.eclipse.swt.SWT;
51 import org.eclipse.swt.widgets.FileDialog;
52 
61 
62 import com.sun.j3d.utils.geometry.GeometryInfo;
63 import com.sun.j3d.utils.geometry.NormalGenerator;
64 import com.sun.j3d.utils.picking.PickTool;
65 
66 
67 @SuppressWarnings("serial") //$NON-NLS-1$
68 public class GrxLinkItem extends GrxTransformItem{
69  private double[] translation_ = {0,0,0}; // parent link local
70  private double[] rotation_ = {0,1,0,0};
71  public short jointId_;
72  public String jointType_;
73  public double jointValue_;
74  public double[] jointAxis_;
75  public double[] ulimit_;
76  public double[] llimit_;
77  private double[] uvlimit_;
78  private double[] lvlimit_;
79  public double linkMass_;
80  private double[] linkCenterOfMass_;
81  private double[] linkInertia_;
82  public double rotorInertia_;
83  public double rotorResistor_;
84  public double gearRatio_;
85  public double torqueConst_;
86  public double encoderPulse_;
87 
88  public short parentIndex_;
89  public short[] childIndices_;
90  private short AABBmaxNum_;
91 
92  // display
93  private Switch switchCom_;
94  private TransformGroup tgCom_;
95  private Switch switchAxis_;
96  private Switch switchAABB_;
97 
98  public Transform3D absTransform(){
99  Transform3D t3d = new Transform3D();
100  tg_.getTransform(t3d);
101  return t3d;
102  }
103 
104  public double[] localTranslation(){
105  return translation_;
106  }
107 
108  public double[] localRotation(){
109  return rotation_;
110  }
111 
112  public void absTransform(double[] p, double[] R){
113  Transform3D t3d = new Transform3D();
114  tg_.getTransform(t3d);
115  t3d.setTranslation(new Vector3d(p));
116  AxisAngle4d a4d = new AxisAngle4d();
117  a4d.setMatrix(new Matrix3d(R));
118  double[] newrot = new double[4];
119  a4d.get(newrot);
120  t3d.setRotation(new AxisAngle4d(newrot));
121  tg_.setTransform(t3d);
122  }
123 
124  public boolean localTranslation(double[] pos){
125  if (pos == null || pos.length != 3) return false;
126  translation_ = pos;
127  setDblAry("translation", pos, 4);
128  if (model_ != null && parent_ != null) model_.notifyModified();
129  return true;
130  }
131 
132  public boolean localTranslation(String value){
133  double [] pos = getDblAry(value);
134  if (localTranslation(pos)){
135  return true;
136  }else{
137  return false;
138  }
139  }
140 
141  public boolean localRotation(double[] rot){
142  if (rot == null) return false;
143  if(rot.length == 9){
144  AxisAngle4d a4d = new AxisAngle4d();
145  a4d.setMatrix(new Matrix3d(rot));
146  rot = new double[4];
147  a4d.get(rot);
148  }
149  if(rot.length == 4){
150  rotation_ = rot;
151  setDblAry("rotation", rot, 4);
152  if (model_ != null && parent_ != null) model_.notifyModified();
153  return true;
154  }
155  return false;
156  }
157 
158  public boolean localRotation(String value){
159  double [] rot = getDblAry(value);
160  if (localRotation(rot)){
161  return true;
162  }else{
163  return false;
164  }
165  }
166 
171  public void jointValue(double jv){
172  jointValue_ = jv;
173  setDbl("angle", jointValue_); //$NON-NLS-1$
174  }
175 
181  public boolean jointValue(String value){
182  Double a = getDbl(value);
183  if (a != null){
184  jointValue(a);
185  return true;
186  }else{
187  return false;
188  }
189  }
190 
196  public boolean inertia(double [] newI){
197  if (newI != null && newI.length == 9){
198  linkInertia_ = newI;
199  setDblAry("momentsOfInertia", linkInertia_); //$NON-NLS-1$
200  _updateScaleOfBall();
201  if (model_ != null) model_.notifyModified();
202  return true;
203  }
204  return false;
205  }
206 
207  public void inertia(String i){
208  double [] mi = getDblAry(i);
209  if(mi != null)
210  inertia(mi);
211  }
212 
218  public boolean centerOfMass(double [] com){
219  if (com != null && com.length==3){
220  linkCenterOfMass_ = com;
221  setDblAry("centerOfMass", com); //$NON-NLS-1$
222  if (model_ != null) model_.notifyModified();
223  Transform3D t3d = new Transform3D();
224  tgCom_.getTransform(t3d);
225  t3d.setTranslation(new Vector3d(linkCenterOfMass_));
226  tgCom_.setTransform(t3d);
227  return true;
228  }else{
229  return false;
230  }
231  }
232 
237  public void centerOfMass(String value){
238  double [] com = getDblAry(value);
239  if(com != null)
240  centerOfMass(com);
241  }
242 
247  public void jointAxis(double[] newAxis){
248  if(jointType_.equals("fixed")||jointType_.equals("free"))
249  return;
250  if (newAxis != null && newAxis.length == 3){
251  jointAxis_ = newAxis;
252  setDblAry("jointAxis", newAxis); //$NON-NLS-1$
253  updateAxis();
254  if (model_ != null) model_.notifyModified();
255  }
256  }
257 
258  void jointAxis(String axis){
259  double[] newAxis = getDblAry(axis);
260  if(newAxis != null)
261  jointAxis(newAxis);
262  }
263 
268  public void jointId(short id){
269  jointId_ = id;
270  setShort("jointId", id); //$NON-NLS-1$
271  if (model_ != null) model_.notifyModified();
272  }
273 
278  public void jointId(String value){
279  Short id = getShort(value);
280  if (id != null && id != jointId_){
281  jointId(id);
282  }
283  }
284 
289  public void mass(double m){
290  linkMass_ = m;
291  setDbl("mass", linkMass_); //$NON-NLS-1$
292  _updateScaleOfBall();
293  if (model_ != null) model_.notifyModified();
294  }
295 
296  public boolean mass(String value){
297  Double a = getDbl(value);
298  if (a != null){
299  mass(a);
300  return true;
301  }else{
302  return false;
303  }
304  }
305 
310  public void jointType(String type){
311  if (type.equals("fixed")||type.equals("rotate")||type.equals("free")||type.equals("slide"))
312  jointType_ = type;
313  else
314  jointType_ = "free";
315  setProperty("jointType", type); //$NON-NLS-1$
316  if(type.equals("fixed")||type.equals("free"))
317  setProperty("jointAxis", "---");
318  else
319  if(getProperty("jointAxis")== null || getProperty("jointAxis").equals("---"))
320  jointAxis("0.0 0.0 1.0");
321  if (model_ != null) model_.notifyModified();
322  }
323 
324  public void gearRatio(double r){
325  gearRatio_ = r;
326  setDbl("gearRatio", r);
327  if (model_ != null) model_.notifyModified();
328  }
329 
330  public void gearRatio(String g){
331  Double gr = getDbl(g);
332  if(gr != null)
333  gearRatio(gr);
334  }
335 
336  public void encoderPulse(double e){
337  encoderPulse_ = e;
338  setDbl("encoderPulse", e);
339  if (model_ != null) model_.notifyModified();
340  }
341 
342  public void encoderPulse(String e){
343  Double ep = getDbl(e);
344  if(ep != null)
345  encoderPulse(ep);
346  }
347 
348  public void rotorInertia(double r){
349  rotorInertia_ = r;
350  setDbl("rotorInertia", r);
351  if (model_ != null) model_.notifyModified();
352  }
353 
354  public void rotorResistor(double r){
355  rotorResistor_ = r;
356  setDbl("rotorResistor", r);
357  if (model_ != null) model_.notifyModified();
358  }
359 
360  public void rotorResistor(String r){
361  Double rr = getDbl(r);
362  if(rr != null)
363  rotorResistor(rr);
364  }
365 
366  public void torqueConst(double t){
367  torqueConst_ = t;
368  setDbl("torqueConst", t);
369  if (model_ != null) model_.notifyModified();
370  }
371 
372  public void torqueConst(String t){
373  Double tc = getDbl(t);
374  if(tc != null)
375  torqueConst(tc);
376  }
377 
378  public void ulimit(double[] u){
379  ulimit_ = u;
380  setDblAry("ulimit", u);
381  if (model_ != null) model_.notifyModified();
382  }
383 
384  public void ulimit(String u){
385  double[] ulimit = getDblAry(u);
386  if(ulimit != null)
387  ulimit(ulimit);
388  }
389 
390  public void llimit(double[] l){
391  llimit_ = l;
392  setDblAry("llimit", l);
393  if (model_ != null) model_.notifyModified();
394  }
395 
396  public void llimit(String l){
397  double[] llimit = getDblAry(l);
398  if(llimit != null)
399  llimit(llimit);
400  }
401 
402  public void uvlimit(double[] uv){
403  uvlimit_ = uv;
404  setDblAry("uvlimit", uv);
405  if (model_ != null) model_.notifyModified();
406  }
407 
408  public void uvlimit(String uv){
409  double[] uvlimit = getDblAry(uv);
410  if(uvlimit != null)
411  uvlimit(uvlimit);
412  }
413 
414  public void lvlimit(double[] lv){
415  lvlimit_ = lv;
416  setDblAry("lvlimit", lv);
417  if (model_ != null) model_.notifyModified();
418  }
419 
420  public void lvlimit(String lv){
421  double[] lvlimit = getDblAry(lv);
422  if(lvlimit != null)
423  lvlimit(lvlimit);
424  }
425 
430  public void addLink(String name){
431  System.out.println("GrxLinkItem.addLink("+name+") is called"); //$NON-NLS-1$ //$NON-NLS-2$
432  try{
433  GrxLinkItem newLink = new GrxLinkItem(name, manager_, model_);
434  addLink(newLink);
435  System.out.println("GrxLinkItem.addLink("+name+") is done"); //$NON-NLS-1$ //$NON-NLS-2$
436  manager_.itemChange(newLink, GrxPluginManager.ADD_ITEM);
437  }catch(Exception ex){
438  ex.printStackTrace();
439  }
440  //manager_.reselectItems();
441  }
442 
447  public void addLink(GrxLinkItem child){
448  children_.add(child);
449  child.parent_ = this;
450  model_.bgRoot_.addChild(child.bg_);
451  child.calcForwardKinematics();
452  }
453 
458  public void addSensor(String name){
459  try{
460  GrxSensorItem sensor = new GrxSensorItem(name, manager_, model_, null);
461  addSensor(sensor);
462  manager_.itemChange(sensor, GrxPluginManager.ADD_ITEM);
463  }catch(Exception ex){
464  ex.printStackTrace();
465  }
466  }
467 
472  public void addSegment(String name){
473  try{
474  GrxSegmentItem segment = new GrxSegmentItem(name, manager_, model_, null, null);
475  addChild(segment);
476  manager_.itemChange(segment, GrxPluginManager.ADD_ITEM);
477  }catch(Exception ex){
478  ex.printStackTrace();
479  }
480  }
481 
486  public void addSensor(GrxSensorItem sensor){
487  addChild(sensor);
488  // TODO : GrxModelItem.sensorMap_ and GrxModelItem.cameraList_ must be updated
489  }
490 
491  public void removeSensor(GrxSensorItem sensor){
492  removeChild(sensor);
493  if (sensor.isCamera()){
494  // TODO : GrxModelItem.sensorMap_ and GrxModelItem.cameraList_ must be updated
495  }
496  }
497 
502  public void addRobot(File f){
503  model_.addRobot(f, this);
504  }
505 
510  public Vector3d absCoM(){
511  Vector3d absCom = new Vector3d(linkCenterOfMass_);
512  return transformV3(absCom);
513  }
514 
515  private void _updateScaleOfBall(){
516  Matrix3d I = new Matrix3d(linkInertia_);
517  double m = linkMass_;
518  Vector3d scale = CalcInertiaUtil.calcScale(I, m);
519  Transform3D t3d = new Transform3D();
520  tgCom_.getTransform(t3d);
521  t3d.setScale(scale);
522  try{
523  tgCom_.setTransform(t3d);
524  }catch(BadTransformException ex){
525  System.out.println("BadTransformException in _updateScaleOfBall"); //$NON-NLS-1$
526  }
527  }
528 
535  public boolean propertyChanged(String property, String value) {
536  if (property.equals("name")){ //$NON-NLS-1$
537  rename(value);
538  }else if (property.equals("angle")){ //$NON-NLS-1$
539  if (jointValue(value)){
540  model_.updateInitialJointValue(this);
541  calcForwardKinematics();
542  }
543  }else if(property.equals("translation")){ //$NON-NLS-1$
544  if (localTranslation(value)){
545  calcForwardKinematics();
546  model_.updateInitialTransformRoot();
547  }
548  }else if(property.equals("rotation")){ //$NON-NLS-1$
549  if (localRotation(value)){
550  calcForwardKinematics();
551  model_.updateInitialTransformRoot();
552  }
553  }else if(property.equals("jointAxis")){ //$NON-NLS-1$
554  jointAxis(value);
555  }else if(property.equals("jointType")){ //$NON-NLS-1$
556  jointType(value);
557  }else if(property.equals("jointId")){ //$NON-NLS-1$
558  jointId(value);
559  }else if(property.equals("ulimit")){ //$NON-NLS-1$
560  ulimit(value);
561  }else if(property.equals("llimit")){ //$NON-NLS-1$
562  llimit(value);
563  }else if(property.equals("uvlimit")){ //$NON-NLS-1$
564  uvlimit(value);
565  }else if(property.equals("lvlimit")){ //$NON-NLS-1$
566  lvlimit(value);
567  }else if(property.equals("torqueConst")){ //$NON-NLS-1$
568  torqueConst(value);
569  }else if(property.equals("rotorResistor")){ //$NON-NLS-1$
570  rotorResistor(value);
571  }else if(property.equals("encoderPulse")){ //$NON-NLS-1$
572  encoderPulse(value);
573  }else if(property.equals("gearRatio")){ //$NON-NLS-1$
574  gearRatio(value);
575  }else if(property.equals("centerOfMass")){ //$NON-NLS-1$
576  centerOfMass(value);
577  }else if(property.equals("mass")){ //$NON-NLS-1$
578  mass(value);
579  }else if(property.equals("momentsOfInertia")){ //$NON-NLS-1$
580  inertia(value);
581  }else if (property.equals("tolerance")){ //$NON-NLS-1$
582  Double tr = getDbl(value);
583  if (tr != null){
584  setProperty("tolerance", value); //$NON-NLS-1$
585  }
586  }else if (property.equals("NumOfAABB")){
587  try{
588  int depth = Integer.parseInt(value);
589  if(depth<AABBmaxNum_){
590  setProperty("NumOfAABB", value);
591  model_.setProperty(getName()+".NumOfAABB", value);
592  model_.makeAABBforSameUrlModels();
593  }else if(AABBmaxNum_==0){
594  setProperty("NumOfAABB", "no shape Data");
595  }
596  }catch(NumberFormatException e){
597  System.out.println("input number");
598  //TODO
599  return false;
600  }
601  }else if( property.equals("mode")){
602  setProperty("mode", value);
603  model_.setProperty(getName()+".mode", value);
604  }else if( property.equals("jointVelocity")){
605  setProperty("jointVelocity", value);
606  model_.setProperty(getName()+".jointVelocity", value);
607  }else {
608  return false;
609  }
610  return true;
611  }
612 
616  public void calcForwardKinematics(){
617  Transform3D t3dp = new Transform3D();
618  Transform3D t3d = new Transform3D();
619  Vector3d v3d = new Vector3d();
620  Vector3d v3d2 = new Vector3d();
621  Matrix3d m3d = new Matrix3d();
622  Matrix3d m3d2 = new Matrix3d();
623  AxisAngle4d a4d = new AxisAngle4d();
624  if (parent_ != null){
625  t3dp = ((GrxLinkItem)parent_).absTransform();
626  v3d.set(localTranslation());
627  if (jointType_.equals("rotate")) { //$NON-NLS-1$ //$NON-NLS-2$
628  t3d.setTranslation(v3d);
629  m3d.set(new AxisAngle4d(localRotation()));
630  a4d.set(jointAxis_[0], jointAxis_[1], jointAxis_[2], jointValue_);
631  m3d2.set(a4d);
632  m3d.mul(m3d2);
633  t3d.setRotation(m3d);
634  } else if(jointType_.equals("slide")) { //$NON-NLS-1$
635  m3d.set(new AxisAngle4d(localRotation()));
636  v3d2.set(jointAxis_[0], jointAxis_[1], jointAxis_[2]);
637  v3d2.scale(jointValue_);
638  m3d.transform(v3d2);
639  v3d.add(v3d2);
640  t3d.setTranslation(v3d);
641  t3d.setRotation(m3d);
642  }else if(jointType_.equals("free") || jointType_.equals("fixed") ){
643  t3d.setTranslation(v3d);
644  m3d.set(new AxisAngle4d(localRotation()));
645  t3d.setRotation(m3d);
646  }
647  t3dp.mul(t3d);
648  tg_.setTransform(t3dp);
649  }else{
650  v3d.set(localTranslation());
651  t3d.setTranslation(v3d);
652  t3d.setRotation(new AxisAngle4d(localRotation()));
653  tg_.setTransform(t3d);
654  }
655  for (int i=0; i<children_.size(); i++){
656  if (children_.get(i) instanceof GrxLinkItem){
657  GrxLinkItem link = (GrxLinkItem)children_.get(i);
658  link.calcForwardKinematics();
659  }
660  }
661 
662  }
663 
664  protected GrxLinkItem(String name, GrxPluginManager manager, GrxModelItem model){
665  super(name, manager, model);
666  _init();
667  jointValue(0);
668  localTranslation(new double[]{0.0, 0.0, 0.0});
669  localRotation(new double[]{0.0, 0.0, 1.0, 0.0});
670  centerOfMass(new double[]{0.0, 0.0, 0.0});
671  inertia(new double[]{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0});
672  jointType("rotate");
673  jointAxis(new double[]{0.0, 0.0, 1.0});
674  mass(0.0);
675  ulimit(new double[]{0,0});
676  llimit(new double[]{0,0});
677  uvlimit(new double[]{0,0});
678  lvlimit(new double[]{0,0});
679  gearRatio(1.0);
680  torqueConst(1.0);
681  rotorInertia(1.0);
682  rotorResistor(1.0);
683  encoderPulse(1.0);
684  jointId((short)-1);
685  parentIndex_ = 0;
686  childIndices_ = null;
687  AABBmaxNum_ = 0;
688  }
693  protected GrxLinkItem(String name, GrxPluginManager manager, GrxModelItem model, LinkInfo info) {
694  super(name, manager, model);
695  _init();
696  jointValue(0);
697  localTranslation(info.translation);
698  localRotation(info.rotation);
699  centerOfMass(info.centerOfMass);
700  inertia(info.inertia);
701  jointType(info.jointType);
702  jointAxis(info.jointAxis);
703  mass(info.mass);
704  if (info.ulimit == null || info.ulimit.length == 0)
705  ulimit(new double[]{0,0});
706  else
707  ulimit(info.ulimit);
708  if (info.llimit == null || info.llimit.length == 0)
709  llimit(new double[]{0,0});
710  else
711  llimit(info.llimit);
712  uvlimit(info.uvlimit);
713  lvlimit(info.lvlimit);
714  gearRatio(info.gearRatio);
715  torqueConst(info.torqueConst);
716  rotorInertia(info.rotorInertia);
717  rotorResistor(info.rotorResistor);
718  encoderPulse(info.encoderPulse);
719  jointId(info.jointId);
720 
721  SensorInfo[] sinfo = info.sensors;
722  if (sinfo != null){
723  for (int i=0; i<sinfo.length; i++) {
724  GrxSensorItem sensor = new GrxSensorItem(sinfo[i].name, manager_, model_, sinfo[i]);
725  manager_.itemChange(sensor, GrxPluginManager.ADD_ITEM);
726  addSensor(sensor);
727  }
728  }
729 
730  HwcInfo[] hinfo = info.hwcs;
731  if (hinfo != null){
732  for (int i=0; i<hinfo.length; i++) {
733  GrxHwcItem hwc = new GrxHwcItem(hinfo[i].name, manager_, model_, hinfo[i]);
734  manager_.itemChange(hwc, GrxPluginManager.ADD_ITEM);
735  addChild(hwc);
736  }
737  }
738 
739  SegmentInfo[] segmentInfo = info.segments;
740  if (segmentInfo != null){
741  for (int i=0; i<segmentInfo.length; i++) {
742  GrxSegmentItem segment = new GrxSegmentItem(segmentInfo[i].name, manager_, model_, info, segmentInfo[i]);
743  manager_.itemChange(segment, GrxPluginManager.ADD_ITEM);
744  addChild(segment);
745  }
746  }
747  parentIndex_ = info.parentIndex;
748  childIndices_ = info.childIndices;
749  AABBmaxNum_ = info.AABBmaxNum;
750  }
751 
755  private void _initMenu(){
756  getMenu().clear();
757 
758  Action item;
759 
760  // rename
761  item = new Action(){
762  public String getText(){
763  return MessageBundle.get("GrxLinkItem.menu.rename"); //$NON-NLS-1$
764  }
765  public void run(){
766  InputDialog dialog = new InputDialog( null, getText(),
767  MessageBundle.get("GrxLinkItem.dialog.message.rename"), getName(),null); //$NON-NLS-1$
768  if ( dialog.open() == InputDialog.OK && dialog.getValue() != null)
769  rename( dialog.getValue() );
770  }
771  };
772  setMenuItem(item);
773 
774  // delete
775  item = new Action(){
776  public String getText(){
777  return MessageBundle.get("GrxLinkItem.menu.delete"); //$NON-NLS-1$
778  }
779  public void run(){
780  String mes = MessageBundle.get("GrxLinkItem.dialog.message.delete"); //$NON-NLS-1$
781  mes = NLS.bind(mes, new String[]{getName()});
782  if(parent_ == null){ // can't delete root link
783  MessageDialog.openInformation(null, MessageBundle.get("GrxLinkItem.dialog.title.delete0"),
784  MessageBundle.get("GrxLinkItem.dialog.message.rootLinkDelete"));
785  return;
786  }
787  if( MessageDialog.openQuestion( null, MessageBundle.get("GrxLinkItem.dialog.title.delete0"), //$NON-NLS-1$
788  mes) )
789  delete();
790  }
791  };
792  setMenuItem(item);
793 
794  // menu item : add joint
795  item = new Action(){
796  public String getText(){
797  return MessageBundle.get("GrxLinkItem.menu.addJoint"); //$NON-NLS-1$
798  }
799  public void run(){
800  InputDialog dialog = new InputDialog( null, getText(),
801  MessageBundle.get("GrxLinkItem.dialog.message.jointName"), null,null); //$NON-NLS-1$
802  if ( dialog.open() == InputDialog.OK && dialog.getValue() != null)
803  addLink( dialog.getValue() );
804  }
805  };
806  setMenuItem(item);
807 
808  // menu item : add sensor
809  item = new Action(){
810  public String getText(){
811  return MessageBundle.get("GrxLinkItem.menu.addSensor"); //$NON-NLS-1$
812  }
813  public void run(){
814  InputDialog dialog = new InputDialog( null, getText(),
815  MessageBundle.get("GrxLinkItem.dialog.message.sensorName"), null,null); //$NON-NLS-1$
816  if ( dialog.open() == InputDialog.OK && dialog.getValue() != null)
817  addSensor( dialog.getValue() );
818  }
819  };
820  setMenuItem(item);
821 
822  // menu item : add segment
823  item = new Action(){
824  public String getText(){
825  return MessageBundle.get("GrxLinkItem.menu.addSegment"); //$NON-NLS-1$
826  }
827  public void run(){
828  InputDialog dialog = new InputDialog( null, getText(),
829  MessageBundle.get("GrxLinkItem.dialog.message.segmentName"), null,null); //$NON-NLS-1$
830  if ( dialog.open() == InputDialog.OK && dialog.getValue() != null)
831  addSegment( dialog.getValue() );
832  }
833  };
834  setMenuItem(item);
835 
836  // menu item : add robot
837  item = new Action(){
838  public String getText(){
839  return MessageBundle.get("GrxLinkItem.menu.addRobot"); //$NON-NLS-1$
840  }
841  public void run(){
842  FileDialog fdlg = new FileDialog(GrxUIPerspectiveFactory.getCurrentShell(), SWT.OPEN);
843  String[] fe = { "*.wrl" };
844  fdlg.setFilterExtensions(fe);
845  String fPath = fdlg.open();
846  if (fPath != null) {
847  File f = new File(fPath);
848  addRobot( f );
849  }
850  }
851  };
852  setMenuItem(item);
853 
854  /* diable copy and paste menus until they are implemented
855  // menu item : copy
856  item = new Action(){
857  public String getText(){
858  return "copy";
859  }
860  public void run(){
861  GrxDebugUtil.println("GrxModelItem.GrxLinkItem copy Action");
862  manager_.setSelectedGrxBaseItemList();
863  }
864  };
865  setMenuItem(item);
866 
867  // menu item : paste
868  item = new Action(){
869  public String getText(){
870  return "paste";
871  }
872  public void run(){
873  }
874  };
875  setMenuItem(item);
876  */
877 
878  }
882  private void _init(){
883  _initMenu();
884 
885  model_.addLink(this);
886 
887  setDbl("tolerance", 0.0); //$NON-NLS-1$
888  setDbl("jointVelocity", 0.0);
889 
890  // CoM display
891  // 0.01 is default scale of ellipsoid
892  switchCom_ = GrxShapeUtil.createBall(0.01, new Color3f(1.0f, 0.5f, 0.5f), 0.5f);
893  tgCom_ = (TransformGroup)switchCom_.getChild(0);
894  tg_.addChild(switchCom_);
895 
896  Transform3D tr = new Transform3D();
897  tr.setIdentity();
898  tg_.setTransform(tr);
899 
901  Vector3d jointAxis = new Vector3d(0.0, 0.0, 1.0);
902  switchAxis_ = SceneGraphModifier._makeSwitchNode(modifier._makeAxisLine(jointAxis));
903  tg_.addChild(switchAxis_);
904 
905  setIcon("joint.png"); //$NON-NLS-1$
906 
907  Map<String, Object> userData = new Hashtable<String, Object>();
908  userData.put("linkInfo", this); //$NON-NLS-1$
909  userData.put("object", model_); //$NON-NLS-1$
910  tg_.setUserData(userData);
911  tg_.setCapability(TransformGroup.ENABLE_PICK_REPORTING);
912 
913  switchAABB_ = SceneGraphModifier._makeSwitchNode();
914  tg_.addChild(switchAABB_);
915  setProperty("NumOfAABB","original data"); //String.valueOf(AABBmaxDepth_));
916  if(model_.getProperty(getName()+".NumOfAABB")!=null)
917  model_.remove(getName()+".NumOfAABB");
918  }
919 
920  private void updateAxis(){
921  try{
922  Shape3D shapeNode = (Shape3D)switchAxis_.getChild(0);
923  Geometry gm = (Geometry)shapeNode.getGeometry(0);
925  Point3f[] p3fW = modifier.makeAxisPoints(new Vector3d(jointAxis_));
926  if (gm instanceof LineArray){
927  LineArray la = (LineArray)gm;
928  la.setCoordinates(0, p3fW);
929  }
930  }catch(Exception ex){
931  ex.printStackTrace();
932  }
933  }
934 
935 
940  public GrxLinkItem clone(){
941  GrxLinkItem ret = (GrxLinkItem)super.clone();
942 /*
943  Deep copy suspension list
944 */
945 
946  return ret;
947  }
948 
954  public void setTransform(Vector3d pos, Matrix3d rot) {
955  if (parent_ != null) return; // root only
956 
957  if (pos != null){
958  double[] newpos = new double[3];
959  pos.get(newpos);
960  localTranslation(newpos);
961  }
962  if (rot != null){
963  AxisAngle4d a4d = new AxisAngle4d();
964  a4d.setMatrix(rot);
965  double[] newrot = new double[4];
966  a4d.get(newrot);
967  localRotation(newrot);
968  }
969  if (pos != null || rot != null) calcForwardKinematics();
970  }
971 
976  public void setTransform(Transform3D trans) {
977  Vector3d pos = new Vector3d();
978  Matrix3d rot = new Matrix3d();
979  trans.get(rot, pos);
980  setTransform(pos, rot);
981  }
982 
987  if (llimit_ != null && ulimit_ != null && llimit_[0] < ulimit_[0]) {
988  if (jointValue_ < llimit_[0])
989  jointValue(llimit_[0]);
990  else if (ulimit_[0] < jointValue_)
991  jointValue(ulimit_[0]);
992  }
993  for (int i=0; i<children_.size(); i++){
994  if (children_.get(i) instanceof GrxLinkItem){
995  GrxLinkItem link = (GrxLinkItem)children_.get(i);
997  }
998  }
999  }
1000 
1007  public void setFocused(boolean b){
1008  //if (b!=isSelected()) System.out.println("GrxLinkItem.setFocused("+getName()+" of "+model_.getName()+", flag = "+b+")");
1009  if (b){
1010  resizeBoundingBox();
1011  if (jointType_.equals("rotate") || jointType_.equals("slide")) { //$NON-NLS-1$ //$NON-NLS-2$
1012  updateAxis();
1013  switchAxis_.setWhichChild(Switch.CHILD_ALL);
1014  }else
1015  switchAxis_.setWhichChild(Switch.CHILD_NONE);
1016  }else{
1017  switchAxis_.setWhichChild(Switch.CHILD_NONE);
1018  }
1019  super.setFocused(b);
1020  switchCom_.setWhichChild(b? Switch.CHILD_ALL:Switch.CHILD_NONE);
1021  }
1022 
1026  public void delete(){
1027  super.delete();
1028  model_.removeLink(this);
1029  }
1030 
1031  public void setColor(java.awt.Color color){
1032  for (int i=0; i<children_.size(); i++){
1033  if (children_.get(i) instanceof GrxShapeItem){
1034  GrxShapeItem shape = (GrxShapeItem)children_.get(i);
1035  shape.setColor(color);
1036  }else if (children_.get(i) instanceof GrxSensorItem){
1037  for (int j=0; j<children_.get(i).children_.size(); j++){
1038  GrxShapeItem shape = (GrxShapeItem)children_.get(i).children_.get(j);
1039  shape.setColor(color);
1040  }
1041  }
1042  }
1043  }
1044 
1045  public void restoreColor(){
1046  for (int i=0; i<children_.size(); i++){
1047  if (children_.get(i) instanceof GrxShapeItem){
1048  GrxShapeItem shape = (GrxShapeItem)children_.get(i);
1049  shape.restoreColor();
1050  }else if (children_.get(i) instanceof GrxSensorItem){
1051  for (int j=0; j<children_.get(i).children_.size(); j++){
1052  GrxShapeItem shape = (GrxShapeItem)children_.get(i).children_.get(j);
1053  shape.restoreColor();
1054  }
1055  }
1056  }
1057  }
1058 
1059  public void setVisibleAABB(boolean b){
1060  switchAABB_.setWhichChild(b? Switch.CHILD_ALL:Switch.CHILD_NONE);
1061  }
1062 
1063  public void clearAABB(){
1064  switchAABB_.removeAllChildren();
1065  }
1066 
1067  public void makeAABB(ShapeInfo shape, double[] T){
1068  TransformGroup tg = new TransformGroup();
1069  tg.setCapability(TransformGroup.ALLOW_CHILDREN_READ);
1070  tg.setCapability(TransformGroup.ALLOW_CHILDREN_EXTEND);
1071  tg.setCapability(TransformGroup.ALLOW_CHILDREN_WRITE);
1072  tg.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
1073  tg.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
1074  Transform3D t3d = new Transform3D();
1075  tg.getTransform(t3d);
1076  Vector3d v = new Vector3d(T[3], T[7], T[11]);
1077  t3d.setTranslation(v);
1078  tg.setTransform(t3d);
1079  Appearance appearance = new Appearance();
1080  PolygonAttributes pa = new PolygonAttributes();
1081  pa.setPolygonMode(PolygonAttributes.POLYGON_LINE);
1082  appearance.setPolygonAttributes(pa);
1083 
1084  GeometryInfo geometryInfo = new GeometryInfo(GeometryInfo.TRIANGLE_ARRAY);
1085  int numVertices = shape.vertices.length / 3;
1086  Point3f[] vertices = new Point3f[numVertices];
1087  for(int i=0; i < numVertices; ++i){
1088  vertices[i] = new Point3f(shape.vertices[i*3], shape.vertices[i*3+1], shape.vertices[i*3+2]);
1089  }
1090  geometryInfo.setCoordinates(vertices);
1091  geometryInfo.setCoordinateIndices(shape.triangles);
1092  NormalGenerator ng = new NormalGenerator();
1093  ng.generateNormals(geometryInfo);
1094 
1095  Shape3D shape3D = new Shape3D(geometryInfo.getGeometryArray());
1096  shape3D.setCapability(Shape3D.ALLOW_APPEARANCE_READ);
1097  shape3D.setCapability(Shape3D.ALLOW_GEOMETRY_READ);
1098  shape3D.setCapability(GeometryArray.ALLOW_COORDINATE_READ);
1099  shape3D.setCapability(GeometryArray.ALLOW_COUNT_READ);
1100  PickTool.setCapabilities(shape3D, PickTool.INTERSECT_FULL);
1101  shape3D.setAppearance(appearance);
1102 
1103  tg.addChild(shape3D);
1104  BranchGroup bg = new BranchGroup();
1105  bg.setCapability(BranchGroup.ALLOW_DETACH);
1106  bg.setCapability(BranchGroup.ALLOW_CHILDREN_READ);
1107  bg.setCapability(BranchGroup.ALLOW_CHILDREN_EXTEND);
1108  bg.addChild(tg);
1109  switchAABB_.addChild(bg);
1110 
1111  }
1112 
1117  public void rename(String newName) {
1118  String oldName = getName();
1119  setName(newName);
1120  if (model_ != null) model_.notifyModified();
1121  OrderedHashMap mcoll = manager_.pluginMap_.get(GrxCollisionPairItem.class);
1122  if(mcoll != null)
1123  {
1124  String modelName = model().getName();
1125  Iterator<?> it = mcoll.values().iterator();
1126  while(it.hasNext())
1127  {
1129  if(modelName.equals(ci.getProperty("objectName1")) && oldName.equals(ci.getProperty("jointName1")))
1130  ci.setProperty("jointName1", newName);
1131 
1132  if(modelName.equals(ci.getProperty("objectName2")) && oldName.equals(ci.getProperty("jointName2")))
1133  ci.setProperty("jointName2", newName);
1134  }
1135  }
1136  }
1137 
1138  @Override
1139  public ValueEditType GetValueEditType(String key) {
1140  if(key.equals("jointType"))
1141  {
1142  return new ValueEditCombo(jointTypeComboItem_);
1143  }else if(key.equals("mode")){
1144  return new ValueEditCombo(modeComboItem_);
1145  }else if( key.equals("mass") || key.equals("centerOfMass") || key.equals("momentsOfInertia") ){
1146  return null;
1147  }
1148  return super.GetValueEditType(key);
1149  }
1150 
1151  public void modifyMass(){
1152  double w = 0.0;
1153  for(int i=0; i<children_.size(); i++){
1154  if(children_.get(i) instanceof GrxSegmentItem){
1155  GrxSegmentItem segment = (GrxSegmentItem)children_.get(i);
1156  w += segment.mass_;
1157  }
1158  }
1159  mass(w);
1160  modifyCenterOfMass();
1161  modifyInertia();
1162  }
1163 
1164  public void modifyCenterOfMass() {
1165  double[] w = {0.0, 0.0, 0.0};
1166  for(int i=0; i<children_.size(); i++){
1167  if(children_.get(i) instanceof GrxSegmentItem){
1168  GrxSegmentItem segment = (GrxSegmentItem)children_.get(i);
1169  Vector3d com = segment.transformV3(new Vector3d(segment.centerOfMass_));
1170  w[0] += segment.mass_ * com.x;
1171  w[1] += segment.mass_ * com.y;
1172  w[2] += segment.mass_ * com.z;
1173 
1174  }
1175  }
1176  for(int j=0; j<3; j++){
1177  if(linkMass_==0.0)
1178  w[j] = 0.0;
1179  else
1180  w[j] /= linkMass_;
1181  }
1182  centerOfMass(w);
1183  modifyInertia();
1184  model_.updateCoM();
1185  }
1186 
1187  public void modifyInertia() {
1188  double[] w = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
1189  for(int i=0; i<children_.size(); i++){
1190  if(children_.get(i) instanceof GrxSegmentItem){
1191  GrxSegmentItem segment = (GrxSegmentItem)children_.get(i);
1192  Matrix3d I = new Matrix3d(segment.momentOfInertia_);
1193  Matrix3d R = new Matrix3d();
1194  segment.getTransform().get(R);
1195  Matrix3d W = new Matrix3d();
1196  W.mul(R,I);
1197  I.mulTransposeRight(W,R);
1198 
1199  Vector3d com = segment.transformV3(new Vector3d(segment.centerOfMass_));
1200  double x = com.x - linkCenterOfMass_[0];
1201  double y = com.y - linkCenterOfMass_[1];
1202  double z = com.z - linkCenterOfMass_[2];
1203  double m = segment.mass_;
1204  w[0] += I.m00 + m * (y*y + z*z);
1205  w[1] += I.m01 - m * x * y;
1206  w[2] += I.m02 - m * x * z;
1207  w[3] += I.m10 - m * y * x;
1208  w[4] += I.m11 + m * (z*z + x*x);
1209  w[5] += I.m12 - m * y * z;
1210  w[6] += I.m20 - m * z * x;
1211  w[7] += I.m21 - m * z * y;
1212  w[8] += I.m22 + m * (x*x + y*y);
1213  }
1214  }
1215  inertia(w);
1216  }
1217 }
void addLink(String name)
create and add a new link as a child
void makeAABB(ShapeInfo shape, double[] T)
void setColor(java.awt.Color color)
static final String get(String key)
GrxLinkItem clone()
Override clone method.
void addRobot(File f)
load and add a new robot as a child
png_infop png_charp png_int_32 png_int_32 int * type
Definition: png.h:2332
Point3f [] makeAxisPoints(Vector3d jointAxis)
make both end points of axis line
void setTransform(Vector3d pos, Matrix3d rot)
set new position and rotation in global frame
* x
Definition: IceUtils.h:98
#define null
our own NULL pointer
Definition: IceTypes.h:57
short [] childIndices_
子リンクインデックス列
Vector3d absCoM()
compute CoM in global frame
png_infop png_charpp name
Definition: png.h:2382
png_voidp int value
Definition: png.h:2113
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
void addSensor(GrxSensorItem sensor)
add a sensor as a child
item corresponds to a robot model
void jointId(short id)
set joint id
void calcForwardKinematics()
compute forward kinematics
void addSegment(String name)
create and add a new sensor as a child
void rename(String newName)
rename this Link
w
png_uint_32 i
Definition: png.h:2735
void setJointValuesWithinLimit()
limit joint value within limits recursively
long b
Definition: jpegint.h:371
png_infop png_bytep * trans
Definition: png.h:2435
void absTransform(double[] p, double[] R)
GrxLinkItem(String name, GrxPluginManager manager, GrxModelItem model, LinkInfo info)
boolean jointValue(String value)
set joint value from string
item which have a transformation
GrxLinkItem(String name, GrxPluginManager manager, GrxModelItem model)
boolean propertyChanged(String property, String value)
check validity of new value of property and update if valid
static Vector3d calcScale(Matrix3d I, double m)
void addLink(GrxLinkItem child)
add a child link
void addChild(GrxTransformItem child)
add a child
void centerOfMass(String value)
set CoM position from string
string a
boolean inertia(double [] newI)
set inertia matrix
def run(tree, args)
boolean localTranslation(String value)
Object setProperty(String key, String value)
set property value associated with a keyword
boolean centerOfMass(double [] com)
set CoM position
backing_store_ptr info
Definition: jmemsys.h:181
void jointType(String type)
set joint type
ValueEditType GetValueEditType(String key)
プラグイン管理クラス GrxUIの核になるクラス。プラグインのロード等の、初期化を実行する。 プラグインとそ...
void jointValue(double jv)
set new joint value
functions to make various basic shapes
def getText(self, nodes=None)
void jointAxis(double[] newAxis)
set joint axis
static Switch createBall(double radius, Color3f c)
create ball with switch node
org
boolean localTranslation(double[] pos)
* y
Definition: IceUtils.h:97
void addSensor(String name)
create and add a new sensor as a child
void _init()
common initialization
void mass(double m)
set mass from string
void setTransform(Transform3D trans)
set new position and rotation in global frame
void setFocused(boolean b)
set/unset fucus on this item
void removeSensor(GrxSensorItem sensor)


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:03