InvKinemaResolver.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  */
00017 package com.generalrobotix.ui.view.tdview;
00018 
00019 import javax.media.j3d.Transform3D;
00020 import javax.media.j3d.TransformGroup;
00021 import javax.vecmath.*;
00022 
00023 import jp.go.aist.hrp.simulator.*;
00024 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.*;
00025 
00026 import com.generalrobotix.ui.GrxPluginManager;
00027 import com.generalrobotix.ui.item.GrxLinkItem;
00028 import com.generalrobotix.ui.item.GrxModelItem;
00029 
00034 public class InvKinemaResolver {
00035     private DynamicsSimulator integrator_;
00036     private GrxPluginManager manager_;
00037     private GrxModelItem robot_;
00038     private GrxLinkItem from_;
00039     private GrxLinkItem to_;
00040     private Transform3D trFrom_;
00041     private LinkPosition tr;
00042 
00050     public InvKinemaResolver(GrxPluginManager manager) {
00051         manager_ = manager;
00052         trFrom_ = new Transform3D();
00053         tr = new LinkPosition();
00054         tr.p = new double[3];
00055         tr.R = new double[9];
00056     }
00057 
00058     public void setDynamicsSimulator(DynamicsSimulator integrator) {
00059         integrator_ = integrator;
00060     }
00061 
00069     public boolean setFromJoint(GrxModelItem model, GrxLinkItem link) {
00070         if (robot_ == null) {
00071             robot_ = model;
00072         } else if(robot_ != model) {
00073             to_ = null;
00074             robot_ = model;
00075         }
00076         
00077         from_ = link;
00078         
00079         // fromジョイントのグローバル座標での位置姿勢を保持
00080         TransformGroup tg = from_.tg_;
00081         Transform3D tr = new Transform3D();
00082         tg.getTransform(tr);
00083         tg.getLocalToVworld(trFrom_);
00084         trFrom_.mul(tr);
00085         //tg.getTransform(trFrom_);
00086 
00087         return true;
00088     }
00089 
00097     public boolean setToJoint(GrxModelItem model, GrxLinkItem link) {
00098         if (robot_ == null) {
00099             robot_ = model;
00100         } else if(robot_ != model) {
00101             return false;
00102         }
00103         
00104         to_ = link;
00105         
00106         return true;
00107     }
00108 
00115     private void _setLinkStatus(String objectName) {
00116         if(robot_ == null) 
00117             robot_ = (GrxModelItem)manager_.getItem(GrxModelItem.class, objectName);
00118         
00119         TransformGroup tg = robot_.getTransformGroupRoot();
00120         Transform3D t3d = new Transform3D();
00121         tg.getTransform(t3d);
00122         Vector3d pos = new Vector3d();
00123         Matrix3d mat = new Matrix3d();
00124         t3d.get(mat, pos);
00125         
00126         double[] value = new double[12];
00127         pos.get(value);
00128         value[3] = mat.m00;
00129         value[4] = mat.m01;
00130         value[5] = mat.m02;
00131         value[6] = mat.m10;
00132         value[7] = mat.m11;
00133         value[8] = mat.m12;
00134         value[9] = mat.m20;
00135         value[10]= mat.m21;
00136         value[11]= mat.m22;
00137 
00138         integrator_.setCharacterLinkData(
00139             objectName, robot_.rootLink().getName(), LinkDataType.ABS_TRANSFORM, value
00140         );
00141         integrator_.setCharacterAllLinkData(
00142             objectName, LinkDataType.JOINT_VALUE, robot_.getJointValues()
00143         );
00144         integrator_.calcCharacterForwardKinematics(objectName);
00145     }
00146 
00155     public boolean resolve(Transform3D transform) {
00156         _setLinkStatus(robot_.getName());
00157         
00158         Matrix3d m3d = new Matrix3d();
00159         Vector3d v3d = new Vector3d();
00160         transform.get(m3d, v3d);
00161         
00162         v3d.get(tr.p);
00163         for (int i=0; i<3; i++) {
00164                 for (int j=0; j<3; j++) {
00165                         tr.R[3*i+j] = m3d.getElement(i,j);
00166                 }
00167         }
00168         try {
00169                         if (robot_ == null || from_ == null || to_ == null)
00170                                 return false;
00171 
00172                 if (!integrator_.calcCharacterInverseKinematics(robot_.getName(), from_.getName(), to_.getName(), tr)) {
00173                         System.out.println("ik failed.");
00174                         robot_.calcForwardKinematics();
00175                 return false;
00176                 };
00177         } catch (Exception e) {
00178                 e.printStackTrace();
00179         }
00180         
00181         DblSequenceHolder v = new DblSequenceHolder();
00182         integrator_.getCharacterAllLinkData(robot_.getName(), LinkDataType.JOINT_VALUE, v);
00183         robot_.setJointValues(v.value);
00184         robot_.setJointValuesWithinLimit();
00185         robot_.updateInitialJointValues();
00186         _setRootJoint(robot_);
00187         robot_.calcForwardKinematics();
00188         
00189         return true;
00190     }
00191     
00192     private void _setRootJoint(GrxModelItem robot) {
00193         Transform3D t3d = new Transform3D();
00194         from_.tg_.getTransform(t3d);
00195         
00196         Transform3D t3dFromNew = new Transform3D();
00197         from_.tg_.getLocalToVworld(t3dFromNew);
00198         t3dFromNew.mul(t3d);
00199         t3dFromNew.invert();
00200         
00201         Transform3D t3dRoot = new Transform3D();
00202         robot.getTransformGroupRoot().getTransform(t3dRoot);
00203         t3d.mul(trFrom_, t3dFromNew);
00204         t3d.mul(t3dRoot);
00205         
00206         Matrix3d rot = new Matrix3d();
00207         Vector3d pos = new Vector3d();
00208         t3d.get(rot, pos);
00209         robot.setTransformRoot(pos, rot);
00210         robot.updateInitialTransformRoot();
00211        /* 
00212         AxisAngle4d  a4d = new AxisAngle4d();
00213         a4d.set(rot);
00214         robot.setProperty(robot_.lInfo_[0].name+".rotation", +a4d.angle+" "+a4d.x+" "+a4d.y+" "+a4d.z);
00215         robot.setProperty(robot_.lInfo_[0].name+".translation", pos.x+" "+pos.y+" "+pos.z);*/
00216     }
00217 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:54