GrxLinkItem.java
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * General Robotix Inc.
00008  * National Institute of Advanced Industrial Science and Technology (AIST)
00009  */
00010 
00011 /*
00012  *  GrxModelItem.java
00013  *
00014  *  @author Yuichiro Kawasumi (General Robotix, Inc.)
00015  *  @author Shin'ichiro Nakaoka (AIST)
00016  */
00017 
00018 package com.generalrobotix.ui.item;
00019 
00020 import java.io.File;
00021 import java.util.Hashtable;
00022 import java.util.Iterator;
00023 import java.util.Map;
00024 
00025 import javax.media.j3d.Appearance;import javax.media.j3d.BadTransformException;
00026 import javax.media.j3d.BranchGroup;
00027 import javax.media.j3d.Geometry;
00028 import javax.media.j3d.GeometryArray;
00029 import javax.media.j3d.LineArray;
00030 import javax.media.j3d.PolygonAttributes;
00031 import javax.media.j3d.Shape3D;
00032 import javax.media.j3d.Switch;
00033 import javax.media.j3d.Transform3D;
00034 import javax.media.j3d.TransformGroup;
00035 import javax.vecmath.Color3f;
00036 import javax.vecmath.Matrix3d;
00037 import javax.vecmath.Point3f;
00038 import javax.vecmath.Vector3d;
00039 
00040 import jp.go.aist.hrp.simulator.HwcInfo;
00041 import jp.go.aist.hrp.simulator.LinkInfo;
00042 import jp.go.aist.hrp.simulator.SensorInfo;
00043 import jp.go.aist.hrp.simulator.ShapeInfo;
00044 import jp.go.aist.hrp.simulator.SegmentInfo;
00045 
00046 import org.eclipse.jface.action.Action;
00047 import org.eclipse.jface.dialogs.InputDialog;
00048 import org.eclipse.jface.dialogs.MessageDialog;
00049 import org.eclipse.osgi.util.NLS;
00050 import org.eclipse.swt.SWT;
00051 import org.eclipse.swt.widgets.FileDialog;
00052 
00053 import com.generalrobotix.ui.GrxPluginManager;
00054 import com.generalrobotix.ui.view.tdview.SceneGraphModifier;
00055 import com.generalrobotix.ui.grxui.GrxUIPerspectiveFactory;
00056 import com.generalrobotix.ui.util.AxisAngle4d;
00057 import com.generalrobotix.ui.util.CalcInertiaUtil;
00058 import com.generalrobotix.ui.util.GrxShapeUtil;
00059 import com.generalrobotix.ui.util.MessageBundle;
00060 import com.generalrobotix.ui.util.OrderedHashMap;
00061 
00062 import com.sun.j3d.utils.geometry.GeometryInfo;
00063 import com.sun.j3d.utils.geometry.NormalGenerator;
00064 import com.sun.j3d.utils.picking.PickTool;
00065 
00066 
00067 @SuppressWarnings("serial") //$NON-NLS-1$
00068 public class GrxLinkItem extends GrxTransformItem{
00069         private double[] translation_ = {0,0,0};                // parent link local 
00070         private double[] rotation_ = {0,1,0,0};
00071     public short        jointId_;
00072     public String jointType_;
00073     public double jointValue_;
00074     public double[] jointAxis_;
00075     public double[] ulimit_;
00076     public double[] llimit_;
00077     private double[] uvlimit_;
00078     private double[] lvlimit_;
00079     public double         linkMass_; 
00080     private double[] linkCenterOfMass_;
00081     private double[] linkInertia_;
00082     public double       rotorInertia_; 
00083     public double               rotorResistor_;
00084     public double               gearRatio_;
00085     public double               torqueConst_;  
00086     public double               encoderPulse_;  
00087 
00088     public short                parentIndex_;  
00089     public short[]      childIndices_; 
00090     private short AABBmaxNum_;
00091 
00092     // display
00093     private Switch switchCom_;
00094     private TransformGroup tgCom_;
00095     private Switch switchAxis_;
00096     private Switch switchAABB_;
00097 
00098     public Transform3D absTransform(){
00099         Transform3D t3d = new Transform3D();
00100         tg_.getTransform(t3d);
00101         return t3d;
00102     }
00103     
00104     public double[] localTranslation(){
00105         return translation_;
00106     }
00107     
00108     public double[] localRotation(){
00109         return rotation_;
00110     }
00111 
00112     public void absTransform(double[] p, double[] R){
00113         Transform3D t3d = new Transform3D();
00114         tg_.getTransform(t3d);        
00115         t3d.setTranslation(new Vector3d(p));
00116         AxisAngle4d a4d = new AxisAngle4d();
00117                 a4d.setMatrix(new Matrix3d(R));
00118                 double[] newrot = new double[4];
00119                 a4d.get(newrot);
00120         t3d.setRotation(new AxisAngle4d(newrot));
00121         tg_.setTransform(t3d);
00122     }
00123     
00124     public boolean localTranslation(double[] pos){
00125         if (pos == null || pos.length != 3) return false;
00126         translation_ = pos;
00127         setDblAry("translation", pos, 4);
00128         if (model_ != null && parent_ != null) model_.notifyModified();
00129         return true;
00130     }
00131     
00132     public boolean localTranslation(String value){
00133         double [] pos = getDblAry(value);
00134         if (localTranslation(pos)){
00135             return true;
00136         }else{
00137                 return false;
00138         }
00139     }
00140     
00141     public boolean localRotation(double[] rot){
00142         if (rot == null)        return false;
00143         if(rot.length == 9){
00144                 AxisAngle4d a4d = new AxisAngle4d();
00145                 a4d.setMatrix(new Matrix3d(rot));
00146                 rot = new double[4];
00147                 a4d.get(rot);
00148         }
00149         if(rot.length == 4){ 
00150                 rotation_ = rot;
00151                 setDblAry("rotation", rot, 4);
00152                 if (model_ != null && parent_ != null) model_.notifyModified();
00153                 return true;
00154         }
00155         return false;
00156     }
00157     
00158     public boolean localRotation(String value){
00159         double [] rot = getDblAry(value);
00160         if (localRotation(rot)){
00161             return true;
00162         }else{
00163                 return false;
00164         }
00165     }
00166  
00171     public void jointValue(double jv){
00172         jointValue_ = jv;
00173         setDbl("angle", jointValue_); //$NON-NLS-1$
00174     }
00175     
00181     public boolean jointValue(String value){
00182                 Double a = getDbl(value);
00183                 if (a != null){
00184                         jointValue(a);
00185                         return true;
00186                 }else{
00187                         return false;
00188                 }
00189     }
00190     
00196     public boolean inertia(double [] newI){
00197         if (newI != null && newI.length == 9){
00198                 linkInertia_ = newI;
00199                 setDblAry("momentsOfInertia", linkInertia_); //$NON-NLS-1$
00200                 _updateScaleOfBall();
00201                 if (model_ != null) model_.notifyModified();
00202                 return true;
00203         }
00204         return false;
00205     }
00206     
00207     public void inertia(String i){
00208         double [] mi = getDblAry(i);
00209         if(mi != null)
00210                 inertia(mi);
00211     }
00212     
00218     public boolean centerOfMass(double [] com){
00219                 if (com != null && com.length==3){
00220                 linkCenterOfMass_ = com;
00221                 setDblAry("centerOfMass", com); //$NON-NLS-1$
00222                 if (model_ != null) model_.notifyModified();
00223                 Transform3D t3d = new Transform3D();
00224                 tgCom_.getTransform(t3d);
00225                 t3d.setTranslation(new Vector3d(linkCenterOfMass_));
00226                 tgCom_.setTransform(t3d);
00227                 return true;
00228                 }else{
00229                         return false;
00230                 }
00231     }
00232     
00237     public void centerOfMass(String value){
00238                 double [] com = getDblAry(value);
00239                 if(com != null)
00240                         centerOfMass(com);
00241     }
00242     
00247     public void jointAxis(double[] newAxis){
00248         if(jointType_.equals("fixed")||jointType_.equals("free"))
00249                 return;
00250         if (newAxis != null && newAxis.length == 3){
00251                 jointAxis_ = newAxis;
00252                 setDblAry("jointAxis", newAxis); //$NON-NLS-1$
00253                 updateAxis();
00254                 if (model_ != null) model_.notifyModified();
00255         }       
00256     }
00257 
00258     void jointAxis(String axis){
00259         double[] newAxis = getDblAry(axis);
00260         if(newAxis != null)
00261                 jointAxis(newAxis);
00262     }
00263  
00268     public void jointId(short id){
00269         jointId_ = id;
00270                 setShort("jointId", id); //$NON-NLS-1$
00271                 if (model_ != null) model_.notifyModified();
00272     }
00273     
00278     public void jointId(String value){
00279         Short id = getShort(value);
00280         if (id != null && id != jointId_){
00281                 jointId(id);
00282         }
00283     }
00284 
00289     public void mass(double m){
00290         linkMass_ = m;
00291         setDbl("mass", linkMass_); //$NON-NLS-1$
00292         _updateScaleOfBall();
00293                 if (model_ != null) model_.notifyModified();
00294     }
00295     
00296     public boolean mass(String value){
00297         Double a = getDbl(value);
00298                 if (a != null){
00299                         mass(a);
00300                         return true;
00301                 }else{
00302                         return false;
00303                 }
00304     }
00305     
00310     public void jointType(String type){
00311                 if (type.equals("fixed")||type.equals("rotate")||type.equals("free")||type.equals("slide"))
00312                 jointType_ = type;
00313                 else
00314                         jointType_ = "free";
00315                 setProperty("jointType", type); //$NON-NLS-1$
00316                 if(type.equals("fixed")||type.equals("free"))
00317                         setProperty("jointAxis", "---");
00318                 else
00319                         if(getProperty("jointAxis")== null || getProperty("jointAxis").equals("---"))
00320                                 jointAxis("0.0 0.0 1.0");
00321                 if (model_ != null) model_.notifyModified();            
00322     }
00323 
00324     public void gearRatio(double r){
00325         gearRatio_ = r;
00326         setDbl("gearRatio", r);
00327                 if (model_ != null) model_.notifyModified();
00328     }
00329     
00330     public void gearRatio(String g){
00331         Double gr = getDbl(g);
00332         if(gr != null)
00333                 gearRatio(gr);
00334     }
00335     
00336     public void encoderPulse(double e){
00337         encoderPulse_ = e;
00338         setDbl("encoderPulse", e);
00339         if (model_ != null) model_.notifyModified();
00340     }
00341 
00342     public void encoderPulse(String e){
00343         Double ep = getDbl(e);
00344         if(ep != null)
00345                 encoderPulse(ep);
00346     }
00347     
00348     public void rotorInertia(double r){
00349         rotorInertia_ = r;
00350         setDbl("rotorInertia", r);
00351         if (model_ != null) model_.notifyModified();
00352     }
00353 
00354     public void rotorResistor(double r){
00355         rotorResistor_ = r;
00356         setDbl("rotorResistor", r);
00357         if (model_ != null) model_.notifyModified();
00358     }
00359 
00360     public void rotorResistor(String r){
00361         Double rr = getDbl(r);
00362         if(rr != null)
00363                 rotorResistor(rr);
00364     }
00365     
00366     public void torqueConst(double t){
00367         torqueConst_ = t;
00368         setDbl("torqueConst", t);
00369         if (model_ != null) model_.notifyModified();
00370     }
00371 
00372     public void torqueConst(String t){
00373         Double tc = getDbl(t);
00374         if(tc != null)
00375                 torqueConst(tc);
00376     }
00377     
00378     public void ulimit(double[] u){
00379         ulimit_ = u;
00380         setDblAry("ulimit", u);
00381         if (model_ != null) model_.notifyModified();
00382     }
00383 
00384     public void ulimit(String u){
00385         double[] ulimit = getDblAry(u);
00386         if(ulimit != null)
00387                 ulimit(ulimit);
00388     }
00389     
00390     public void llimit(double[] l){
00391         llimit_ = l;
00392         setDblAry("llimit", l);
00393         if (model_ != null) model_.notifyModified();
00394     }
00395 
00396     public void llimit(String l){
00397         double[] llimit = getDblAry(l);
00398         if(llimit != null)
00399                 llimit(llimit);
00400     }
00401     
00402     public void uvlimit(double[] uv){
00403         uvlimit_ = uv;
00404         setDblAry("uvlimit", uv);
00405         if (model_ != null) model_.notifyModified();
00406     }
00407     
00408     public void uvlimit(String uv){
00409         double[] uvlimit = getDblAry(uv);
00410         if(uvlimit != null)
00411                 uvlimit(uvlimit);
00412     }
00413     
00414     public void lvlimit(double[] lv){
00415         lvlimit_ = lv;
00416         setDblAry("lvlimit", lv);
00417         if (model_ != null) model_.notifyModified();
00418     }
00419     
00420     public void lvlimit(String lv){
00421         double[] lvlimit = getDblAry(lv);
00422         if(lvlimit != null)
00423                 lvlimit(lvlimit);
00424     }
00425         
00430     public void addLink(String name){
00431         System.out.println("GrxLinkItem.addLink("+name+") is called"); //$NON-NLS-1$ //$NON-NLS-2$
00432         try{
00433                 GrxLinkItem newLink = new GrxLinkItem(name, manager_, model_);
00434                 addLink(newLink);
00435                 System.out.println("GrxLinkItem.addLink("+name+") is done"); //$NON-NLS-1$ //$NON-NLS-2$
00436                 manager_.itemChange(newLink, GrxPluginManager.ADD_ITEM);
00437         }catch(Exception ex){
00438                 ex.printStackTrace();
00439         }
00440         //manager_.reselectItems();
00441     }
00442 
00447     public void addLink(GrxLinkItem child){
00448         children_.add(child);
00449         child.parent_ = this;
00450         model_.bgRoot_.addChild(child.bg_);
00451         child.calcForwardKinematics();
00452     }
00453 
00458     public void addSensor(String name){
00459         try{
00460                 GrxSensorItem sensor = new GrxSensorItem(name, manager_, model_, null);
00461                 addSensor(sensor);
00462                 manager_.itemChange(sensor, GrxPluginManager.ADD_ITEM);
00463         }catch(Exception ex){
00464                 ex.printStackTrace();
00465         }
00466     }
00467 
00472     public void addSegment(String name){
00473         try{
00474                 GrxSegmentItem segment = new GrxSegmentItem(name, manager_, model_, null, null);
00475                 addChild(segment);
00476                  manager_.itemChange(segment, GrxPluginManager.ADD_ITEM);
00477         }catch(Exception ex){
00478                 ex.printStackTrace();
00479         }
00480     }
00481 
00486     public void addSensor(GrxSensorItem sensor){
00487         addChild(sensor);
00488         // TODO : GrxModelItem.sensorMap_ and GrxModelItem.cameraList_ must be updated
00489     }
00490 
00491     public void removeSensor(GrxSensorItem sensor){
00492         removeChild(sensor);
00493         if (sensor.isCamera()){
00494                 // TODO : GrxModelItem.sensorMap_ and GrxModelItem.cameraList_ must be updated
00495         }
00496     }
00497     
00502     public void addRobot(File f){
00503         model_.addRobot(f, this);
00504     }
00505     
00510     public Vector3d absCoM(){
00511         Vector3d absCom = new Vector3d(linkCenterOfMass_);
00512         return transformV3(absCom);
00513     }
00514        
00515     private void _updateScaleOfBall(){
00516                 Matrix3d I = new Matrix3d(linkInertia_);
00517                 double m = linkMass_;
00518                 Vector3d scale = CalcInertiaUtil.calcScale(I, m);
00519                 Transform3D t3d = new Transform3D();
00520                 tgCom_.getTransform(t3d);
00521                 t3d.setScale(scale);
00522                 try{
00523                         tgCom_.setTransform(t3d);
00524                 }catch(BadTransformException ex){
00525                         System.out.println("BadTransformException in _updateScaleOfBall"); //$NON-NLS-1$
00526                 }
00527     }
00528     
00535     public boolean propertyChanged(String property, String value) {
00536         if (property.equals("name")){ //$NON-NLS-1$
00537                         rename(value);
00538         }else if (property.equals("angle")){ //$NON-NLS-1$
00539                 if (jointValue(value)){
00540                         model_.updateInitialJointValue(this);
00541                         calcForwardKinematics();
00542                 }
00543         }else if(property.equals("translation")){ //$NON-NLS-1$
00544                 if (localTranslation(value)){
00545                         calcForwardKinematics();
00546                         model_.updateInitialTransformRoot();
00547                 }
00548         }else if(property.equals("rotation")){ //$NON-NLS-1$
00549                 if (localRotation(value)){
00550                 calcForwardKinematics();
00551                         model_.updateInitialTransformRoot();
00552                 }
00553         }else if(property.equals("jointAxis")){ //$NON-NLS-1$
00554                 jointAxis(value);
00555         }else if(property.equals("jointType")){ //$NON-NLS-1$
00556                 jointType(value);
00557         }else if(property.equals("jointId")){ //$NON-NLS-1$
00558                 jointId(value);
00559         }else if(property.equals("ulimit")){ //$NON-NLS-1$
00560                 ulimit(value);
00561         }else if(property.equals("llimit")){ //$NON-NLS-1$
00562                 llimit(value);
00563         }else if(property.equals("uvlimit")){ //$NON-NLS-1$
00564                 uvlimit(value);
00565         }else if(property.equals("lvlimit")){ //$NON-NLS-1$
00566                 lvlimit(value);
00567         }else if(property.equals("torqueConst")){ //$NON-NLS-1$
00568                 torqueConst(value);
00569         }else if(property.equals("rotorResistor")){ //$NON-NLS-1$
00570                 rotorResistor(value);
00571         }else if(property.equals("encoderPulse")){ //$NON-NLS-1$
00572                 encoderPulse(value);
00573         }else if(property.equals("gearRatio")){ //$NON-NLS-1$
00574                 gearRatio(value);
00575         }else if(property.equals("centerOfMass")){ //$NON-NLS-1$
00576                 centerOfMass(value);
00577         }else if(property.equals("mass")){ //$NON-NLS-1$
00578                 mass(value);
00579         }else if(property.equals("momentsOfInertia")){ //$NON-NLS-1$
00580                 inertia(value);
00581         }else if (property.equals("tolerance")){ //$NON-NLS-1$
00582                 Double tr = getDbl(value);
00583                 if (tr != null){
00584                         setProperty("tolerance", value); //$NON-NLS-1$
00585                 }
00586         }else if (property.equals("NumOfAABB")){
00587                 try{
00588                         int depth = Integer.parseInt(value);
00589                         if(depth<AABBmaxNum_){
00590                                 setProperty("NumOfAABB", value);
00591                                 model_.setProperty(getName()+".NumOfAABB", value);
00592                                 model_.makeAABBforSameUrlModels();
00593                         }else if(AABBmaxNum_==0){
00594                                 setProperty("NumOfAABB", "no shape Data");
00595                         }
00596                 }catch(NumberFormatException e){
00597                         System.out.println("input number");
00598                         //TODO
00599                         return false;
00600                 }
00601         }else if( property.equals("mode")){
00602                 setProperty("mode", value);
00603                 model_.setProperty(getName()+".mode", value);
00604         }else if( property.equals("jointVelocity")){
00605                 setProperty("jointVelocity", value);
00606                 model_.setProperty(getName()+".jointVelocity", value);
00607         }else {
00608                 return false;
00609         }
00610         return true;
00611     }
00612 
00616     public void calcForwardKinematics(){
00617         Transform3D t3dp = new Transform3D();
00618         Transform3D t3d = new Transform3D();
00619         Vector3d v3d = new Vector3d();
00620         Vector3d v3d2 = new Vector3d();
00621         Matrix3d m3d = new Matrix3d();
00622         Matrix3d m3d2 = new Matrix3d();
00623         AxisAngle4d a4d = new AxisAngle4d();
00624         if (parent_ != null){
00625                 t3dp = ((GrxLinkItem)parent_).absTransform();
00626             v3d.set(localTranslation());
00627             if (jointType_.equals("rotate")) { //$NON-NLS-1$ //$NON-NLS-2$
00628                 t3d.setTranslation(v3d);
00629                 m3d.set(new AxisAngle4d(localRotation()));
00630                 a4d.set(jointAxis_[0], jointAxis_[1], jointAxis_[2], jointValue_);
00631                 m3d2.set(a4d);
00632                 m3d.mul(m3d2);
00633                 t3d.setRotation(m3d);
00634             } else if(jointType_.equals("slide")) { //$NON-NLS-1$
00635                 m3d.set(new AxisAngle4d(localRotation()));
00636                 v3d2.set(jointAxis_[0], jointAxis_[1], jointAxis_[2]);
00637                 v3d2.scale(jointValue_);
00638                 m3d.transform(v3d2);
00639                 v3d.add(v3d2);
00640                 t3d.setTranslation(v3d);
00641                 t3d.setRotation(m3d);
00642             }else if(jointType_.equals("free") || jointType_.equals("fixed") ){
00643                 t3d.setTranslation(v3d);
00644                 m3d.set(new AxisAngle4d(localRotation()));
00645                 t3d.setRotation(m3d);
00646             }
00647             t3dp.mul(t3d);
00648             tg_.setTransform(t3dp);
00649         }else{
00650                 v3d.set(localTranslation());
00651                 t3d.setTranslation(v3d);
00652                 t3d.setRotation(new AxisAngle4d(localRotation()));
00653                 tg_.setTransform(t3d);
00654         }
00655         for (int i=0; i<children_.size(); i++){
00656                 if (children_.get(i) instanceof GrxLinkItem){
00657                         GrxLinkItem link = (GrxLinkItem)children_.get(i);
00658                         link.calcForwardKinematics();
00659                 }
00660         }
00661 
00662     }
00663 
00664     protected GrxLinkItem(String name, GrxPluginManager manager, GrxModelItem model){
00665         super(name, manager, model);
00666         _init();
00667                 jointValue(0);
00668         localTranslation(new double[]{0.0, 0.0, 0.0});
00669         localRotation(new double[]{0.0, 0.0, 1.0, 0.0});
00670         centerOfMass(new double[]{0.0, 0.0, 0.0});
00671         inertia(new double[]{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0});
00672         jointType("rotate");
00673         jointAxis(new double[]{0.0, 0.0, 1.0});
00674         mass(0.0);
00675         ulimit(new double[]{0,0});
00676         llimit(new double[]{0,0});
00677         uvlimit(new double[]{0,0});
00678         lvlimit(new double[]{0,0});
00679         gearRatio(1.0);
00680         torqueConst(1.0);
00681         rotorInertia(1.0);
00682         rotorResistor(1.0);
00683         encoderPulse(1.0);
00684         jointId((short)-1);
00685         parentIndex_ = 0;  
00686         childIndices_ = null;  
00687         AABBmaxNum_ = 0;
00688     }
00693         protected GrxLinkItem(String name, GrxPluginManager manager, GrxModelItem model, LinkInfo info) {
00694                 super(name, manager, model);
00695                 _init();
00696                 jointValue(0);
00697         localTranslation(info.translation);
00698         localRotation(info.rotation);
00699         centerOfMass(info.centerOfMass);
00700         inertia(info.inertia);
00701         jointType(info.jointType);
00702         jointAxis(info.jointAxis);
00703         mass(info.mass);
00704         if (info.ulimit == null || info.ulimit.length == 0)
00705                 ulimit(new double[]{0,0});
00706         else
00707                 ulimit(info.ulimit);
00708         if (info.llimit == null || info.llimit.length == 0) 
00709                 llimit(new double[]{0,0});
00710         else
00711                 llimit(info.llimit);
00712         uvlimit(info.uvlimit);
00713         lvlimit(info.lvlimit);
00714         gearRatio(info.gearRatio);
00715         torqueConst(info.torqueConst);
00716         rotorInertia(info.rotorInertia);
00717         rotorResistor(info.rotorResistor);
00718         encoderPulse(info.encoderPulse);
00719         jointId(info.jointId);
00720 
00721         SensorInfo[] sinfo = info.sensors;
00722         if (sinfo != null){
00723             for (int i=0; i<sinfo.length; i++) {
00724                 GrxSensorItem sensor = new GrxSensorItem(sinfo[i].name, manager_, model_, sinfo[i]);
00725                 manager_.itemChange(sensor, GrxPluginManager.ADD_ITEM);
00726                 addSensor(sensor);
00727             }
00728         }
00729         
00730         HwcInfo[] hinfo = info.hwcs;
00731         if (hinfo != null){
00732                 for (int i=0; i<hinfo.length; i++) {
00733                         GrxHwcItem hwc = new GrxHwcItem(hinfo[i].name, manager_, model_, hinfo[i]);
00734                         manager_.itemChange(hwc, GrxPluginManager.ADD_ITEM);
00735                         addChild(hwc);
00736                 }
00737         }
00738         
00739         SegmentInfo[] segmentInfo = info.segments;
00740         if (segmentInfo != null){
00741                 for (int i=0; i<segmentInfo.length; i++) {
00742                         GrxSegmentItem segment = new GrxSegmentItem(segmentInfo[i].name, manager_, model_, info, segmentInfo[i]);
00743                         manager_.itemChange(segment, GrxPluginManager.ADD_ITEM);
00744                         addChild(segment);
00745                 }
00746         }
00747         parentIndex_ = info.parentIndex;  
00748         childIndices_ = info.childIndices;  
00749         AABBmaxNum_ = info.AABBmaxNum;
00750     }
00751 
00755         private void _initMenu(){
00756                 getMenu().clear();
00757 
00758                 Action item;
00759 
00760                 // rename
00761                 item = new Action(){
00762                         public String getText(){
00763                                 return MessageBundle.get("GrxLinkItem.menu.rename"); //$NON-NLS-1$
00764                         }
00765                         public void run(){
00766                                 InputDialog dialog = new InputDialog( null, getText(),
00767                                                 MessageBundle.get("GrxLinkItem.dialog.message.rename"), getName(),null); //$NON-NLS-1$
00768                                 if ( dialog.open() == InputDialog.OK && dialog.getValue() != null)
00769                                         rename( dialog.getValue() );
00770                         }
00771                 };
00772                 setMenuItem(item);
00773 
00774                 // delete
00775                 item = new Action(){
00776                         public String getText(){
00777                                 return MessageBundle.get("GrxLinkItem.menu.delete"); //$NON-NLS-1$
00778                         }
00779                         public void run(){
00780                 String mes = MessageBundle.get("GrxLinkItem.dialog.message.delete"); //$NON-NLS-1$
00781                 mes = NLS.bind(mes, new String[]{getName()});
00782                 if(parent_ == null){    // can't delete root link 
00783                         MessageDialog.openInformation(null, MessageBundle.get("GrxLinkItem.dialog.title.delete0"), 
00784                                         MessageBundle.get("GrxLinkItem.dialog.message.rootLinkDelete"));
00785                         return;
00786                 }
00787                                 if( MessageDialog.openQuestion( null, MessageBundle.get("GrxLinkItem.dialog.title.delete0"), //$NON-NLS-1$
00788                                                 mes) )
00789                                         delete();
00790                         }
00791                 };
00792                 setMenuItem(item);
00793 
00794                 // menu item : add joint
00795                 item = new Action(){
00796                         public String getText(){
00797                                 return MessageBundle.get("GrxLinkItem.menu.addJoint"); //$NON-NLS-1$
00798                         }
00799                         public void run(){
00800                                 InputDialog dialog = new InputDialog( null, getText(),
00801                                                 MessageBundle.get("GrxLinkItem.dialog.message.jointName"), null,null); //$NON-NLS-1$
00802                                 if ( dialog.open() == InputDialog.OK && dialog.getValue() != null)
00803                                         addLink( dialog.getValue() );
00804                         }
00805                 };
00806                 setMenuItem(item);
00807 
00808                 // menu item : add sensor
00809                 item = new Action(){
00810                         public String getText(){
00811                                 return MessageBundle.get("GrxLinkItem.menu.addSensor"); //$NON-NLS-1$
00812                         }
00813                         public void run(){
00814                                 InputDialog dialog = new InputDialog( null, getText(),
00815                                                 MessageBundle.get("GrxLinkItem.dialog.message.sensorName"), null,null); //$NON-NLS-1$
00816                                 if ( dialog.open() == InputDialog.OK && dialog.getValue() != null)
00817                                         addSensor( dialog.getValue() );
00818                         }
00819                 };
00820                 setMenuItem(item);
00821 
00822                 // menu item : add segment
00823                 item = new Action(){
00824                         public String getText(){
00825                                 return MessageBundle.get("GrxLinkItem.menu.addSegment"); //$NON-NLS-1$
00826                         }
00827                         public void run(){
00828                                 InputDialog dialog = new InputDialog( null, getText(),
00829                                                 MessageBundle.get("GrxLinkItem.dialog.message.segmentName"), null,null); //$NON-NLS-1$
00830                                 if ( dialog.open() == InputDialog.OK && dialog.getValue() != null)
00831                                         addSegment( dialog.getValue() );
00832                         }
00833                 };
00834                 setMenuItem(item);
00835                 
00836                 // menu item : add robot
00837                 item = new Action(){
00838                         public String getText(){
00839                                 return MessageBundle.get("GrxLinkItem.menu.addRobot"); //$NON-NLS-1$
00840                         }
00841                         public void run(){
00842                                 FileDialog fdlg = new FileDialog(GrxUIPerspectiveFactory.getCurrentShell(), SWT.OPEN);
00843                 String[] fe = { "*.wrl" };
00844                 fdlg.setFilterExtensions(fe);
00845                 String fPath = fdlg.open();
00846                 if (fPath != null) {
00847                     File f = new File(fPath);
00848                                         addRobot( f );
00849                 }
00850                         }
00851                 };
00852                 setMenuItem(item);
00853 
00854         /* diable copy and paste menus until they are implemented
00855         // menu item : copy
00856         item = new Action(){
00857             public String getText(){
00858                 return "copy";
00859             }
00860             public void run(){
00861                 GrxDebugUtil.println("GrxModelItem.GrxLinkItem copy Action");
00862                 manager_.setSelectedGrxBaseItemList();
00863             }
00864         };
00865         setMenuItem(item);
00866 
00867         // menu item : paste
00868         item = new Action(){
00869             public String getText(){
00870                 return "paste";
00871             }
00872             public void run(){
00873             }
00874         };
00875                 setMenuItem(item);
00876                 */
00877 
00878         }
00882         private void _init(){
00883                 _initMenu();
00884                 
00885                 model_.addLink(this);
00886 
00887                 setDbl("tolerance", 0.0); //$NON-NLS-1$
00888         setDbl("jointVelocity", 0.0);
00889                 
00890                 // CoM display
00891                 // 0.01 is default scale of ellipsoid
00892         switchCom_ = GrxShapeUtil.createBall(0.01, new Color3f(1.0f, 0.5f, 0.5f), 0.5f);
00893         tgCom_ = (TransformGroup)switchCom_.getChild(0);
00894         tg_.addChild(switchCom_);
00895 
00896         Transform3D tr = new Transform3D();
00897         tr.setIdentity();
00898         tg_.setTransform(tr);
00899 
00900         SceneGraphModifier modifier = SceneGraphModifier.getInstance();
00901         Vector3d jointAxis = new Vector3d(0.0, 0.0, 1.0);
00902         switchAxis_ = SceneGraphModifier._makeSwitchNode(modifier._makeAxisLine(jointAxis));
00903         tg_.addChild(switchAxis_);
00904 
00905         setIcon("joint.png"); //$NON-NLS-1$
00906 
00907         Map<String, Object> userData = new Hashtable<String, Object>();
00908         userData.put("linkInfo", this); //$NON-NLS-1$
00909         userData.put("object", model_); //$NON-NLS-1$
00910         tg_.setUserData(userData);
00911         tg_.setCapability(TransformGroup.ENABLE_PICK_REPORTING);
00912         
00913         switchAABB_ = SceneGraphModifier._makeSwitchNode();
00914         tg_.addChild(switchAABB_);
00915         setProperty("NumOfAABB","original data"); //String.valueOf(AABBmaxDepth_));
00916         if(model_.getProperty(getName()+".NumOfAABB")!=null)
00917                 model_.remove(getName()+".NumOfAABB");
00918         }
00919         
00920         private void updateAxis(){
00921                 try{
00922                         Shape3D shapeNode = (Shape3D)switchAxis_.getChild(0);
00923                         Geometry gm = (Geometry)shapeNode.getGeometry(0);
00924                         SceneGraphModifier modifier = SceneGraphModifier.getInstance();
00925                         Point3f[] p3fW = modifier.makeAxisPoints(new Vector3d(jointAxis_));
00926                         if (gm instanceof LineArray){
00927                                 LineArray la = (LineArray)gm;
00928                                 la.setCoordinates(0, p3fW);
00929                         }
00930                 }catch(Exception ex){
00931                 ex.printStackTrace();
00932         }
00933         }
00934         
00935         
00940         public GrxLinkItem clone(){
00941                 GrxLinkItem ret = (GrxLinkItem)super.clone();
00942 /*      
00943         Deep copy suspension list
00944 */
00945                 
00946                 return ret;
00947         }
00948 
00954         public void setTransform(Vector3d pos, Matrix3d rot) {
00955                 if (parent_ != null) return;    // root only
00956                 
00957         if (pos != null){
00958                 double[] newpos = new double[3];
00959                 pos.get(newpos);
00960                 localTranslation(newpos);
00961         }
00962         if (rot != null){
00963                 AxisAngle4d a4d = new AxisAngle4d();
00964                 a4d.setMatrix(rot);
00965                 double[] newrot = new double[4];
00966                 a4d.get(newrot);
00967                 localRotation(newrot);
00968         }
00969         if (pos != null || rot != null) calcForwardKinematics();
00970         }
00971 
00976         public void setTransform(Transform3D trans) {
00977                 Vector3d pos = new Vector3d();
00978                 Matrix3d rot = new Matrix3d();
00979                 trans.get(rot, pos);
00980                 setTransform(pos, rot);
00981         }
00982 
00986         public void setJointValuesWithinLimit() {
00987         if (llimit_ != null && ulimit_ != null && llimit_[0] < ulimit_[0]) {
00988             if (jointValue_ < llimit_[0])
00989                 jointValue(llimit_[0]);
00990             else if (ulimit_[0] < jointValue_)
00991                 jointValue(ulimit_[0]);
00992         }
00993         for (int i=0; i<children_.size(); i++){
00994                 if (children_.get(i) instanceof GrxLinkItem){
00995                         GrxLinkItem link = (GrxLinkItem)children_.get(i);
00996                         link.setJointValuesWithinLimit();
00997                 }
00998         }
00999         }
01000     
01007     public void setFocused(boolean b){
01008         //if (b!=isSelected()) System.out.println("GrxLinkItem.setFocused("+getName()+" of "+model_.getName()+", flag = "+b+")");
01009         if (b){
01010                 resizeBoundingBox();
01011                 if (jointType_.equals("rotate") || jointType_.equals("slide")) { //$NON-NLS-1$ //$NON-NLS-2$
01012                         updateAxis();
01013                         switchAxis_.setWhichChild(Switch.CHILD_ALL);
01014                 }else
01015                         switchAxis_.setWhichChild(Switch.CHILD_NONE);
01016         }else{
01017             switchAxis_.setWhichChild(Switch.CHILD_NONE);
01018         }
01019         super.setFocused(b);
01020         switchCom_.setWhichChild(b? Switch.CHILD_ALL:Switch.CHILD_NONE);
01021     }
01022 
01026         public void delete(){
01027                 super.delete();
01028                 model_.removeLink(this);
01029         }
01030         
01031         public void setColor(java.awt.Color color){
01032                 for (int i=0; i<children_.size(); i++){
01033                         if (children_.get(i) instanceof GrxShapeItem){
01034                                 GrxShapeItem shape = (GrxShapeItem)children_.get(i);
01035                                 shape.setColor(color);
01036                         }else if (children_.get(i) instanceof GrxSensorItem){
01037                                 for (int j=0; j<children_.get(i).children_.size(); j++){
01038                                         GrxShapeItem shape = (GrxShapeItem)children_.get(i).children_.get(j);
01039                                         shape.setColor(color);
01040                                 }
01041                         }
01042                 }
01043     }
01044         
01045         public void restoreColor(){
01046                 for (int i=0; i<children_.size(); i++){
01047                         if (children_.get(i) instanceof GrxShapeItem){
01048                                 GrxShapeItem shape = (GrxShapeItem)children_.get(i);
01049                                 shape.restoreColor();
01050                         }else if (children_.get(i) instanceof GrxSensorItem){
01051                                 for (int j=0; j<children_.get(i).children_.size(); j++){
01052                                         GrxShapeItem shape = (GrxShapeItem)children_.get(i).children_.get(j);
01053                                         shape.restoreColor();
01054                                 }
01055                         }
01056                 }
01057     }
01058         
01059         public void setVisibleAABB(boolean b){ 
01060                 switchAABB_.setWhichChild(b? Switch.CHILD_ALL:Switch.CHILD_NONE);
01061         }
01062         
01063         public void clearAABB(){
01064                 switchAABB_.removeAllChildren();
01065         }
01066         
01067         public void makeAABB(ShapeInfo shape, double[] T){      
01068                 TransformGroup tg = new TransformGroup();
01069                 tg.setCapability(TransformGroup.ALLOW_CHILDREN_READ);
01070             tg.setCapability(TransformGroup.ALLOW_CHILDREN_EXTEND);
01071             tg.setCapability(TransformGroup.ALLOW_CHILDREN_WRITE);
01072             tg.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
01073             tg.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
01074                 Transform3D t3d = new Transform3D();
01075             tg.getTransform(t3d);
01076             Vector3d v = new Vector3d(T[3], T[7], T[11]);
01077             t3d.setTranslation(v);
01078             tg.setTransform(t3d);
01079             Appearance appearance = new Appearance();
01080             PolygonAttributes pa = new PolygonAttributes();
01081             pa.setPolygonMode(PolygonAttributes.POLYGON_LINE);
01082             appearance.setPolygonAttributes(pa);
01083             
01084             GeometryInfo geometryInfo = new GeometryInfo(GeometryInfo.TRIANGLE_ARRAY);
01085         int numVertices = shape.vertices.length / 3;
01086         Point3f[] vertices = new Point3f[numVertices];
01087         for(int i=0; i < numVertices; ++i){
01088             vertices[i] = new Point3f(shape.vertices[i*3], shape.vertices[i*3+1], shape.vertices[i*3+2]);
01089         }
01090         geometryInfo.setCoordinates(vertices);
01091         geometryInfo.setCoordinateIndices(shape.triangles);
01092         NormalGenerator ng = new NormalGenerator();
01093         ng.generateNormals(geometryInfo);
01094 
01095         Shape3D shape3D = new Shape3D(geometryInfo.getGeometryArray());
01096         shape3D.setCapability(Shape3D.ALLOW_APPEARANCE_READ);
01097         shape3D.setCapability(Shape3D.ALLOW_GEOMETRY_READ);
01098         shape3D.setCapability(GeometryArray.ALLOW_COORDINATE_READ);
01099         shape3D.setCapability(GeometryArray.ALLOW_COUNT_READ);
01100         PickTool.setCapabilities(shape3D, PickTool.INTERSECT_FULL);
01101         shape3D.setAppearance(appearance);
01102             
01103             tg.addChild(shape3D);
01104             BranchGroup bg = new BranchGroup();
01105             bg.setCapability(BranchGroup.ALLOW_DETACH);
01106             bg.setCapability(BranchGroup.ALLOW_CHILDREN_READ);
01107             bg.setCapability(BranchGroup.ALLOW_CHILDREN_EXTEND);
01108             bg.addChild(tg);
01109             switchAABB_.addChild(bg);           
01110 
01111         }
01112 
01117         public void rename(String newName) {
01118         String oldName = getName();
01119         setName(newName);
01120         if (model_ != null) model_.notifyModified();
01121         OrderedHashMap mcoll = manager_.pluginMap_.get(GrxCollisionPairItem.class);
01122         if(mcoll != null)
01123         {
01124                 String modelName = model().getName();
01125                 Iterator<?> it = mcoll.values().iterator();
01126                 while(it.hasNext())
01127                 {
01128                         GrxCollisionPairItem ci = (GrxCollisionPairItem)it.next();
01129                         if(modelName.equals(ci.getProperty("objectName1")) && oldName.equals(ci.getProperty("jointName1")))
01130                                 ci.setProperty("jointName1", newName);
01131 
01132                         if(modelName.equals(ci.getProperty("objectName2")) && oldName.equals(ci.getProperty("jointName2")))
01133                                 ci.setProperty("jointName2", newName);
01134                 }
01135         }
01136         }
01137     
01138     @Override
01139     public ValueEditType GetValueEditType(String key) {
01140         if(key.equals("jointType"))
01141         {
01142             return new ValueEditCombo(jointTypeComboItem_);
01143         }else if(key.equals("mode")){
01144             return new ValueEditCombo(modeComboItem_);
01145         }else if( key.equals("mass") || key.equals("centerOfMass") || key.equals("momentsOfInertia") ){
01146                 return null;
01147         }
01148         return super.GetValueEditType(key);
01149     }
01150 
01151     public void modifyMass(){
01152         double w = 0.0;
01153         for(int i=0; i<children_.size(); i++){
01154                 if(children_.get(i) instanceof GrxSegmentItem){
01155                         GrxSegmentItem segment = (GrxSegmentItem)children_.get(i);
01156                         w += segment.mass_;
01157                 }
01158         }
01159         mass(w);
01160         modifyCenterOfMass();
01161         modifyInertia();
01162     }
01163     
01164         public void modifyCenterOfMass() {
01165                 double[] w = {0.0, 0.0, 0.0};
01166         for(int i=0; i<children_.size(); i++){
01167                 if(children_.get(i) instanceof GrxSegmentItem){
01168                         GrxSegmentItem segment = (GrxSegmentItem)children_.get(i);
01169                         Vector3d com = segment.transformV3(new Vector3d(segment.centerOfMass_));
01170                         w[0] += segment.mass_ * com.x;
01171                         w[1] += segment.mass_ * com.y;
01172                         w[2] += segment.mass_ * com.z;
01173                         
01174                 }
01175         }
01176         for(int j=0; j<3; j++){
01177                 if(linkMass_==0.0)
01178                         w[j] = 0.0;
01179                 else
01180                         w[j] /= linkMass_;
01181         }
01182         centerOfMass(w);
01183         modifyInertia();
01184         model_.updateCoM();
01185         }
01186 
01187         public void modifyInertia() {
01188                 double[] w = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
01189                 for(int i=0; i<children_.size(); i++){
01190                 if(children_.get(i) instanceof GrxSegmentItem){
01191                         GrxSegmentItem segment = (GrxSegmentItem)children_.get(i);
01192                         Matrix3d I = new Matrix3d(segment.momentOfInertia_);
01193                         Matrix3d R = new Matrix3d(); 
01194                         segment.getTransform().get(R);
01195                         Matrix3d W = new Matrix3d(); 
01196                         W.mul(R,I);
01197                         I.mulTransposeRight(W,R);                       
01198                         
01199                         Vector3d com = segment.transformV3(new Vector3d(segment.centerOfMass_));
01200                     double x = com.x - linkCenterOfMass_[0];
01201                         double y = com.y - linkCenterOfMass_[1];
01202                         double z = com.z - linkCenterOfMass_[2];
01203                         double m = segment.mass_;
01204                         w[0] += I.m00 +  m * (y*y + z*z);
01205                         w[1] += I.m01 - m * x * y;
01206                         w[2] += I.m02 - m * x * z;
01207                         w[3] += I.m10 - m * y * x;
01208                         w[4] += I.m11 + m * (z*z + x*x);
01209                         w[5] += I.m12 - m * y * z;
01210                         w[6] += I.m20 - m * z * x;
01211                         w[7] += I.m21 - m * z * y;
01212                         w[8] += I.m22 + m * (x*x + y*y);
01213                 }
01214                 }
01215                 inertia(w);
01216         }
01217 }

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:16