/** * 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); }