Ejemplo n.º 1
0
  /**
   * setLinkStatus
   *
   * <p>Debugようにこのメソッドを叩くと現在のRobotの状態をIntegratorに通知する
   *
   * @param objectName
   */
  private void _setLinkStatus(String objectName) {
    if (robot_ == null) robot_ = (GrxModelItem) manager_.getItem(GrxModelItem.class, objectName);

    TransformGroup tg = robot_.getTransformGroupRoot();
    Transform3D t3d = new Transform3D();
    tg.getTransform(t3d);
    Vector3d pos = new Vector3d();
    Matrix3d mat = new Matrix3d();
    t3d.get(mat, pos);

    double[] value = new double[12];
    pos.get(value);
    value[3] = mat.m00;
    value[4] = mat.m01;
    value[5] = mat.m02;
    value[6] = mat.m10;
    value[7] = mat.m11;
    value[8] = mat.m12;
    value[9] = mat.m20;
    value[10] = mat.m21;
    value[11] = mat.m22;

    integrator_.setCharacterLinkData(
        objectName, robot_.rootLink().getName(), LinkDataType.ABS_TRANSFORM, value);
    integrator_.setCharacterAllLinkData(
        objectName, LinkDataType.JOINT_VALUE, robot_.getJointValues());
    integrator_.calcCharacterForwardKinematics(objectName);
  }
Ejemplo n.º 2
0
  /**
   * 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;
  }