/** * resolve * * <p>マウスの動きから生成された T3D を逆運動学サーバを使って 各ジョイントの動きに直し設定する * * @param transform * @return */ public boolean resolve(Transform3D transform) { _setLinkStatus(robot_.getName()); Matrix3d m3d = new Matrix3d(); Vector3d v3d = new Vector3d(); transform.get(m3d, v3d); v3d.get(tr.p); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { tr.R[3 * i + j] = m3d.getElement(i, j); } } try { if (robot_ == null || from_ == null || to_ == null) return false; if (!integrator_.calcCharacterInverseKinematics( robot_.getName(), from_.getName(), to_.getName(), tr)) { System.out.println("ik failed."); robot_.calcForwardKinematics(); return false; } ; } catch (Exception e) { e.printStackTrace(); } DblSequenceHolder v = new DblSequenceHolder(); integrator_.getCharacterAllLinkData(robot_.getName(), LinkDataType.JOINT_VALUE, v); robot_.setJointValues(v.value); robot_.setJointValuesWithinLimit(); robot_.updateInitialJointValues(); _setRootJoint(robot_); robot_.calcForwardKinematics(); return true; }