18 package com.generalrobotix.ui.item;
21 import java.util.Hashtable;
22 import java.util.Iterator;
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;
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;
62 import com.sun.j3d.utils.geometry.GeometryInfo;
63 import com.sun.j3d.utils.geometry.NormalGenerator;
64 import com.sun.j3d.utils.picking.PickTool;
67 @SuppressWarnings(
"serial")
69 private double[] translation_ = {0,0,0};
70 private double[] rotation_ = {0,1,0,0};
99 Transform3D t3d =
new Transform3D();
100 tg_.getTransform(t3d);
113 Transform3D t3d =
new Transform3D();
114 tg_.getTransform(t3d);
115 t3d.setTranslation(
new Vector3d(p));
118 double[] newrot =
new double[4];
121 tg_.setTransform(t3d);
125 if (pos ==
null || pos.length != 3)
return false;
127 setDblAry(
"translation", pos, 4);
128 if (model_ !=
null && parent_ !=
null) model_.notifyModified();
133 double [] pos = getDblAry(
value);
134 if (localTranslation(pos)){
142 if (rot ==
null)
return false;
151 setDblAry(
"rotation", rot, 4);
152 if (model_ !=
null && parent_ !=
null) model_.notifyModified();
159 double [] rot = getDblAry(
value);
160 if (localRotation(rot)){
173 setDbl(
"angle", jointValue_);
197 if (newI !=
null && newI.length == 9){
199 setDblAry(
"momentsOfInertia", linkInertia_);
200 _updateScaleOfBall();
201 if (model_ !=
null) model_.notifyModified();
208 double [] mi = getDblAry(
i);
219 if (
com !=
null &&
com.length==3){
220 linkCenterOfMass_ =
com;
221 setDblAry(
"centerOfMass",
com);
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);
248 if(jointType_.equals(
"fixed")||jointType_.equals(
"free"))
250 if (newAxis !=
null && newAxis.length == 3){
251 jointAxis_ = newAxis;
252 setDblAry(
"jointAxis", newAxis);
254 if (model_ !=
null) model_.notifyModified();
258 void jointAxis(String axis){
259 double[] newAxis = getDblAry(axis);
270 setShort(
"jointId",
id);
271 if (model_ !=
null) model_.notifyModified();
279 Short
id = getShort(
value);
280 if (
id !=
null &&
id != jointId_){
291 setDbl(
"mass", linkMass_);
292 _updateScaleOfBall();
293 if (model_ !=
null) model_.notifyModified();
311 if (
type.equals(
"fixed")||
type.equals(
"rotate")||
type.equals(
"free")||
type.equals(
"slide"))
315 setProperty(
"jointType",
type);
316 if(
type.equals(
"fixed")||
type.equals(
"free"))
317 setProperty(
"jointAxis",
"---");
319 if(getProperty(
"jointAxis")==
null || getProperty(
"jointAxis").equals(
"---"))
320 jointAxis(
"0.0 0.0 1.0");
321 if (model_ !=
null) model_.notifyModified();
326 setDbl(
"gearRatio", r);
327 if (model_ !=
null) model_.notifyModified();
331 Double gr = getDbl(g);
338 setDbl(
"encoderPulse", e);
339 if (model_ !=
null) model_.notifyModified();
343 Double ep = getDbl(e);
350 setDbl(
"rotorInertia", r);
351 if (model_ !=
null) model_.notifyModified();
356 setDbl(
"rotorResistor", r);
357 if (model_ !=
null) model_.notifyModified();
361 Double rr = getDbl(r);
368 setDbl(
"torqueConst", t);
369 if (model_ !=
null) model_.notifyModified();
373 Double tc = getDbl(t);
380 setDblAry(
"ulimit", u);
381 if (model_ !=
null) model_.notifyModified();
385 double[] ulimit = getDblAry(u);
392 setDblAry(
"llimit", l);
393 if (model_ !=
null) model_.notifyModified();
397 double[] llimit = getDblAry(l);
404 setDblAry(
"uvlimit", uv);
405 if (model_ !=
null) model_.notifyModified();
409 double[] uvlimit = getDblAry(uv);
416 setDblAry(
"lvlimit", lv);
417 if (model_ !=
null) model_.notifyModified();
421 double[] lvlimit = getDblAry(lv);
431 System.out.println(
"GrxLinkItem.addLink("+
name+
") is called");
435 System.out.println(
"GrxLinkItem.addLink("+
name+
") is done");
437 }
catch(Exception ex){
438 ex.printStackTrace();
448 children_.add(child);
463 }
catch(Exception ex){
464 ex.printStackTrace();
477 }
catch(Exception ex){
478 ex.printStackTrace();
503 model_.addRobot(
f,
this);
511 Vector3d absCom =
new Vector3d(linkCenterOfMass_);
512 return transformV3(absCom);
516 Matrix3d I =
new Matrix3d(linkInertia_);
517 double m = linkMass_;
519 Transform3D t3d =
new Transform3D();
520 tgCom_.getTransform(t3d);
523 tgCom_.setTransform(t3d);
524 }
catch(BadTransformException ex){
525 System.out.println(
"BadTransformException in _updateScaleOfBall");
536 if (property.equals(
"name")){
538 }
else if (property.equals(
"angle")){
539 if (jointValue(
value)){
540 model_.updateInitialJointValue(
this);
541 calcForwardKinematics();
543 }
else if(property.equals(
"translation")){
544 if (localTranslation(
value)){
545 calcForwardKinematics();
546 model_.updateInitialTransformRoot();
548 }
else if(property.equals(
"rotation")){
549 if (localRotation(
value)){
550 calcForwardKinematics();
551 model_.updateInitialTransformRoot();
553 }
else if(property.equals(
"jointAxis")){
555 }
else if(property.equals(
"jointType")){
557 }
else if(property.equals(
"jointId")){
559 }
else if(property.equals(
"ulimit")){
561 }
else if(property.equals(
"llimit")){
563 }
else if(property.equals(
"uvlimit")){
565 }
else if(property.equals(
"lvlimit")){
567 }
else if(property.equals(
"torqueConst")){
569 }
else if(property.equals(
"rotorResistor")){
570 rotorResistor(
value);
571 }
else if(property.equals(
"encoderPulse")){
573 }
else if(property.equals(
"gearRatio")){
575 }
else if(property.equals(
"centerOfMass")){
577 }
else if(property.equals(
"mass")){
579 }
else if(property.equals(
"momentsOfInertia")){
581 }
else if (property.equals(
"tolerance")){
582 Double tr = getDbl(
value);
584 setProperty(
"tolerance",
value);
586 }
else if (property.equals(
"NumOfAABB")){
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");
596 }
catch(NumberFormatException e){
597 System.out.println(
"input number");
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);
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();
624 if (parent_ !=
null){
626 v3d.set(localTranslation());
627 if (jointType_.equals(
"rotate")) {
628 t3d.setTranslation(v3d);
630 a4d.set(jointAxis_[0], jointAxis_[1], jointAxis_[2], jointValue_);
633 t3d.setRotation(m3d);
634 }
else if(jointType_.equals(
"slide")) {
636 v3d2.set(jointAxis_[0], jointAxis_[1], jointAxis_[2]);
637 v3d2.scale(jointValue_);
640 t3d.setTranslation(v3d);
641 t3d.setRotation(m3d);
642 }
else if(jointType_.equals(
"free") || jointType_.equals(
"fixed") ){
643 t3d.setTranslation(v3d);
645 t3d.setRotation(m3d);
648 tg_.setTransform(t3dp);
650 v3d.set(localTranslation());
651 t3d.setTranslation(v3d);
653 tg_.setTransform(t3d);
655 for (
int i=0;
i<children_.size();
i++){
665 super(
name, manager, model);
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});
673 jointAxis(
new double[]{0.0, 0.0, 1.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});
686 childIndices_ =
null;
694 super(
name, manager, model);
697 localTranslation(
info.translation);
698 localRotation(
info.rotation);
699 centerOfMass(
info.centerOfMass);
700 inertia(
info.inertia);
701 jointType(
info.jointType);
702 jointAxis(
info.jointAxis);
704 if (
info.ulimit ==
null ||
info.ulimit.length == 0)
705 ulimit(
new double[]{0,0});
708 if (
info.llimit ==
null ||
info.llimit.length == 0)
709 llimit(
new double[]{0,0});
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);
721 SensorInfo[] sinfo =
info.sensors;
723 for (
int i=0;
i<sinfo.length;
i++) {
730 HwcInfo[] hinfo =
info.hwcs;
732 for (
int i=0;
i<hinfo.length;
i++) {
739 SegmentInfo[] segmentInfo =
info.segments;
740 if (segmentInfo !=
null){
741 for (
int i=0;
i<segmentInfo.length;
i++) {
747 parentIndex_ =
info.parentIndex;
748 childIndices_ =
info.childIndices;
749 AABBmaxNum_ =
info.AABBmaxNum;
762 public String getText(){
766 InputDialog dialog =
new InputDialog(
null, getText(),
768 if ( dialog.open() == InputDialog.OK && dialog.getValue() !=
null)
769 rename( dialog.getValue() );
776 public String getText(){
781 mes = NLS.bind(mes,
new String[]{getName()});
783 MessageDialog.openInformation(
null,
MessageBundle.
get(
"GrxLinkItem.dialog.title.delete0"),
787 if( MessageDialog.openQuestion(
null,
MessageBundle.
get(
"GrxLinkItem.dialog.title.delete0"),
796 public String getText(){
800 InputDialog dialog =
new InputDialog(
null, getText(),
802 if ( dialog.open() == InputDialog.OK && dialog.getValue() !=
null)
803 addLink( dialog.getValue() );
810 public String getText(){
814 InputDialog dialog =
new InputDialog(
null, getText(),
816 if ( dialog.open() == InputDialog.OK && dialog.getValue() !=
null)
817 addSensor( dialog.getValue() );
824 public String getText(){
828 InputDialog dialog =
new InputDialog(
null, getText(),
830 if ( dialog.open() == InputDialog.OK && dialog.getValue() !=
null)
831 addSegment( dialog.getValue() );
838 public String getText(){
843 String[] fe = {
"*.wrl" };
844 fdlg.setFilterExtensions(fe);
845 String fPath = fdlg.open();
847 File
f =
new File(fPath);
885 model_.addLink(
this);
887 setDbl(
"tolerance", 0.0);
888 setDbl(
"jointVelocity", 0.0);
893 tgCom_ = (TransformGroup)switchCom_.getChild(0);
894 tg_.addChild(switchCom_);
896 Transform3D tr =
new Transform3D();
898 tg_.setTransform(tr);
901 Vector3d jointAxis =
new Vector3d(0.0, 0.0, 1.0);
903 tg_.addChild(switchAxis_);
905 setIcon(
"joint.png");
907 Map<String, Object> userData =
new Hashtable<String, Object>();
908 userData.put(
"linkInfo",
this);
909 userData.put(
"object", model_);
910 tg_.setUserData(userData);
911 tg_.setCapability(TransformGroup.ENABLE_PICK_REPORTING);
914 tg_.addChild(switchAABB_);
915 setProperty(
"NumOfAABB",
"original data");
916 if(model_.getProperty(getName()+
".NumOfAABB")!=
null)
917 model_.remove(getName()+
".NumOfAABB");
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);
930 }
catch(Exception ex){
931 ex.printStackTrace();
955 if (parent_ !=
null)
return;
958 double[] newpos =
new double[3];
960 localTranslation(newpos);
965 double[] newrot =
new double[4];
967 localRotation(newrot);
969 if (pos !=
null || rot !=
null) calcForwardKinematics();
977 Vector3d pos =
new Vector3d();
978 Matrix3d rot =
new Matrix3d();
980 setTransform(pos, rot);
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]);
993 for (
int i=0;
i<children_.size();
i++){
1010 resizeBoundingBox();
1011 if (jointType_.equals(
"rotate") || jointType_.equals(
"slide")) {
1013 switchAxis_.setWhichChild(Switch.CHILD_ALL);
1015 switchAxis_.setWhichChild(Switch.CHILD_NONE);
1017 switchAxis_.setWhichChild(Switch.CHILD_NONE);
1019 super.setFocused(
b);
1020 switchCom_.setWhichChild(
b? Switch.CHILD_ALL:Switch.CHILD_NONE);
1026 public void delete(){
1028 model_.removeLink(
this);
1032 for (
int i=0;
i<children_.size();
i++){
1035 shape.setColor(color);
1037 for (
int j=0; j<children_.get(
i).children_.size(); j++){
1039 shape.setColor(color);
1046 for (
int i=0;
i<children_.size();
i++){
1049 shape.restoreColor();
1051 for (
int j=0; j<children_.get(
i).children_.size(); j++){
1053 shape.restoreColor();
1060 switchAABB_.setWhichChild(
b? Switch.CHILD_ALL:Switch.CHILD_NONE);
1064 switchAABB_.removeAllChildren();
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);
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]);
1090 geometryInfo.setCoordinates(vertices);
1091 geometryInfo.setCoordinateIndices(shape.triangles);
1092 NormalGenerator ng =
new NormalGenerator();
1093 ng.generateNormals(geometryInfo);
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);
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);
1109 switchAABB_.addChild(bg);
1118 String oldName = getName();
1120 if (model_ !=
null) model_.notifyModified();
1124 String modelName = model().getName();
1125 Iterator<?> it = mcoll.
values().iterator();
1129 if(modelName.equals(ci.getProperty(
"objectName1")) && oldName.equals(ci.getProperty(
"jointName1")))
1132 if(modelName.equals(ci.getProperty(
"objectName2")) && oldName.equals(ci.getProperty(
"jointName2")))
1140 if(key.equals(
"jointType"))
1143 }
else if(key.equals(
"mode")){
1145 }
else if( key.equals(
"mass") || key.equals(
"centerOfMass") || key.equals(
"momentsOfInertia") ){
1148 return super.GetValueEditType(key);
1153 for(
int i=0;
i<children_.size();
i++){
1160 modifyCenterOfMass();
1165 double[] w = {0.0, 0.0, 0.0};
1166 for(
int i=0;
i<children_.size();
i++){
1176 for(
int j=0; j<3; j++){
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++){
1193 Matrix3d R =
new Matrix3d();
1195 Matrix3d W =
new Matrix3d();
1197 I.mulTransposeRight(W,R);
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);