Question

我正在为我的未来项目创建Java逆运动学库。目前，我已经实现了逆向运动学，并且在进行正向计算时具有基于前一个关节的局部旋转的旋转约束。我还添加了长度限制，允许关节长度在某些限制之间变化。除非我将约束设置得很小，或者根本不允许关节旋转，否则这在大多数用例中都可以正常工作。

这是一个JS小提琴： https://jsfiddle.net/jrj2211/413yuabt/4/

class Joint {
  constructor(length) {
    this.position = new THREE.Vector2(0, 0);
    this.effectorPosition = new THREE.Vector2(0, 0);
    this.next = null;
    this.prev = null;
    this.length = length;
    this.angleConstraints = null;
    this.lengthLimits = null;
    this.direction = new THREE.Vector2(1, 0);
    this.update();
  }

  appendJoint(joint) {
    var last = this.getEndJoint();
    last.next = joint;
    joint.prev = last;
    joint.update();
    return joint;
  }

  update() {
    if (this.prev) {
      this.position = this.prev.effectorPosition;
    }
    this.localPosition = this.direction.clone().multiplyScalar(this.length);

    this.effectorPosition = this.position.clone();
    console.log(this.localPosition, this.effectorPosition);
    this.effectorPosition.add(this.localPosition);
  }

  setRootPosition(position) {
    this.position = position;
    this.update();
  }

  backward() {
    // Make copies of the current start and end locations
    var p1 = this.effectorPosition.clone();
    var p2 = this.position.clone();

    // Allow the length to shrink or expand if set
    if (this.lengthLimits) {
      var magnitude = p1.distanceTo(p2);
      this.length = THREE.Math.clamp(magnitude, this.lengthLimits[0], this.lengthLimits[1]);
    }

    // Get the new end position
    p2.sub(p1);
    p2.normalize();
    p2.multiplyScalar(this.length);

    // Set the new start position
    this.position = this.effectorPosition.clone();
    this.position.add(p2);

    // Check if there is a joint before this one
    if (this.prev) {
      // Update the direction for the previous join for use in angle constraints
      if (this.prev.fixed !== true) {
        this.prev.direction = p2.normalize();
      }

      this.prev.effectorPosition = this.position.clone();
      this.prev.backward();
    }
  }

  forward() {
    // Make copies of the current start and end locations
    var p1 = this.position.clone();
    var p2 = this.effectorPosition.clone();

    // Calculate the new start point based on angle and length
    p2.sub(p1);
    p2.normalize();
    p2.multiplyScalar(this.length);

    // Check within angular constraints
    var curAngle = p2.angle();
    var dirAngle = this.direction.angle();
    var localRotation = THREE.Math.radToDeg(curAngle - dirAngle);

    // Check that the angle is within its constraints
    if (this.angleConstraints != null && !this.isBetween(this.angleConstraints[0], this.angleConstraints[1], localRotation)) {
      // Not within angle constraints so find which angle its closest to
      if (localRotation < 0) {
        localRotation += 360;
      }
      var closest = this.closerConstraintAngle(this.angleConstraints[0], this.angleConstraints[1], localRotation) + THREE.Math.radToDeg(this.direction.angle());

      // Set the angle to the closest constraint angle
      this.effectorPosition = this.position.clone();
      this.effectorPosition.x += this.length * Math.cos(THREE.Math.degToRad(closest));
      this.effectorPosition.y += this.length * Math.sin(THREE.Math.degToRad(closest));

      // Update the position
      var p1 = new THREE.Vector2(this.position.x, this.position.y);
      var p2 = new THREE.Vector2(this.effectorPosition.x, this.effectorPosition.y);
      p2.sub(p1);
    } else {
      // No angle constraints or within bounds so update position
      this.effectorPosition.x = p2.x + p1.x;
      this.effectorPosition.y = p2.y + p1.y;
    }

    if (this.next) {
      // Set next position to this effector position
      this.next.position = this.effectorPosition.clone();
      // Update the next joints direction
      this.next.direction = p2.normalize();
      this.next.forward();
    }
  }

  solve() {
    var start = this.position.clone();
    var endJoint = this.getEndJoint();

    // Solve inverse kinematics
    endJoint.effectorPosition.x = this.target.x;
    endJoint.effectorPosition.y = this.target.y;
    endJoint.backward();

    // Reset the start position if its fixed and do forward kinematics
    if (this.fixed === true && this.prev == null) {
      this.position = start;
      this.forward();
    }
  }

  normalizeAngle(angle) {
    return (angle % 360 + 360) % 360;
  }

  isBetween(start, end, mid) {
    mid = Math.abs(mid);
    end = (end - start) < 0 ? end - start + 360 : end - start;
    mid = (mid - start) < 0 ? mid - start + 360 : mid - start;
    return (mid < end);
  }

  closerConstraintAngle(start, end, mid) {
    var end2 = (end - start) < 0 ? end - start + 360 : end - start;
    var mid2 = (mid - start) < 0 ? mid - start + 360 : mid - start;

    var startDistance = 360 - mid2;
    var endDistance = mid2 - end2;

    if (startDistance < endDistance) {
      return start;
    } else {
      return end;
    }
  }

  getEndJoint() {
    let end = this;
    while (end.next != null) {
      end = end.next;
    }
    return end;
  }

  setTarget(target) {
    this.target = target;

    this.solve();
  }

}

// Setup kinematics with just angular rotation constraints
var kinematics1 = new Joint(75);
kinematics1.fixed = true;
kinematics1.setRootPosition(new THREE.Vector2(200, 400));
kinematics1.angleConstraints = [315, 45];
var joint2 = kinematics1.appendJoint(new Joint(75));
var joint3 = kinematics1.appendJoint(new Joint(75));
joint3.angleConstraints = [0, 0];
kinematics1.setTarget(new THREE.Vector2(0, 0));

// Setup kinematics with just angular rotation constraints
var kinematics2 = new Joint(75);
kinematics2.fixed = true;
kinematics2.setRootPosition(new THREE.Vector2(800, 400));
kinematics2.angleConstraints = [315, 45];
kinematics2.direction = new THREE.Vector2(-1, 0);
var joint2 = kinematics2.appendJoint(new Joint(75));
joint2.angleConstraints = [315, 45];
var joint3 = kinematics2.appendJoint(new Joint(75));
joint3.angleConstraints = [315, 45];
kinematics2.setTarget(new THREE.Vector2(0, 0));

// Create Canvas
var canvas = document.getElementById("canvas");
var ctx = canvas.getContext("2d");

// Set target on mouse click
var mouse = {};
canvas.addEventListener('mousedown', function(event) {
  mouse.x = event.clientX;
  mouse.y = event.clientY;

  var x = mouse.x - canvas.offsetLeft + window.scrollX;
  var y = mouse.y - canvas.offsetTop + window.scrollY;

  kinematics1.setTarget(new THREE.Vector2(x, y));
  kinematics2.setTarget(new THREE.Vector2(x, y));
}, false);

function animate() {
  // clear canvas
  ctx.clearRect(0, 0, canvas.width, canvas.height);

  // Draw objects
  draw(kinematics1, true);
  draw(kinematics2, true);
  drawTarget();

  window.requestAnimationFrame(animate);
}

function resizeCanvas() {
  canvas.width = window.innerWidth || document.documentElement.clientWidth || document.body.clientWidth;
  canvas.height = window.innerHeight || document.documentElement.clientHeight || document.body.clientHeight;
}

function draw(kinematics, debug) {
  let cur = kinematics;
  while (cur != null) {

    // Draw Bone
    ctx.beginPath();
    ctx.moveTo(cur.position.x, cur.position.y);
    ctx.lineTo(cur.effectorPosition.x, cur.effectorPosition.y);
    ctx.closePath();
    ctx.lineWidth = 10;
    ctx.strokeStyle = "black";
    ctx.stroke();

    // Draw constraints
    if (cur.angleConstraints && debug) {
      var rotationAngle = cur.direction.angle();
      var startAngle = rotationAngle + THREE.Math.degToRad(cur.angleConstraints[0]);
      var endAngle = rotationAngle + THREE.Math.degToRad(cur.angleConstraints[1]);

      var startConstraintPos = cur.position.clone();
      startConstraintPos.x += 30 * Math.cos(startAngle);
      startConstraintPos.y += 30 * Math.sin(startAngle);

      var endConstraintPos = cur.position.clone();
      endConstraintPos.x += 30 * Math.cos(endAngle);
      endConstraintPos.y += 30 * Math.sin(endAngle);

      ctx.beginPath();
      ctx.moveTo(cur.position.x, cur.position.y);
      ctx.lineTo(startConstraintPos.x, startConstraintPos.y);
      ctx.closePath();
      ctx.strokeStyle = "red";
      ctx.lineWidth = 2;
      ctx.stroke();

      ctx.beginPath();
      ctx.moveTo(cur.position.x, cur.position.y);
      ctx.lineTo(endConstraintPos.x, endConstraintPos.y);
      ctx.closePath();
      ctx.strokeStyle = "blue";
      ctx.lineWidth = 2;
      ctx.stroke();
    }

    if (debug) {
      var dir = cur.direction.clone().multiplyScalar(30).add(cur.position);
      ctx.beginPath();
      ctx.moveTo(cur.position.x, cur.position.y);
      ctx.lineTo(dir.x, dir.y);
      ctx.closePath();
      ctx.strokeStyle = "purple";
      ctx.lineWidth = 2;
      ctx.stroke();
    }

    // Draw Joint
    ctx.beginPath();
    ctx.arc(cur.position.x, cur.position.y, 8, 0, 2 * Math.PI);
    if (cur.fixed === true) {
      ctx.fillStyle = "red";
    } else {
      ctx.fillStyle = "gray";
    }
    ctx.fill();
    ctx.closePath();


    cur = cur.next;
  }
}

function drawTarget() {
  if (kinematics1.target) {
    ctx.beginPath();
    ctx.moveTo(kinematics1.target.x - 10, kinematics1.target.y - 10);
    ctx.lineTo(kinematics1.target.x - 10, kinematics1.target.y + 10);
    ctx.lineTo(kinematics1.target.x + 10, kinematics1.target.y + 10);
    ctx.lineTo(kinematics1.target.x + 10, kinematics1.target.y - 10);
    ctx.closePath();
    ctx.fillStyle = "green";
    ctx.fill();
  }
}

window.addEventListener('resize', resizeCanvas);
resizeCanvas();
animate();

在该演示中，我运行了两个运动学模型，一个运动模型的所有角度都限制为-45和45，另一个运动关节的旋转角度为0。 45度角模型效果很好，但是如果它偏离先前关节局部旋转的轴，则具有0个自由度的模型即使可以，也永远不会到达目标。参见下图。

我知道这是因为我目前仅在算法的前部计算旋转约束。向后计算的最后一个关节接触目标，但是向前计算将其向回捕捉到约束范围之内（可以在上面的“ Whats Happening”图像中看到）。

我知道我可能需要在向后计算中强制执行旋转约束，但是我不确定该怎么做。我正在使用前一个关节的局部旋转作为旋转约束，因为我试图模拟伺服器，它们的约束将相对于前一个关节。

在前向通过上强制角度约束是有意义的，因为您已经有了上一个关节的方向。向后移动时，当更新先前的关节位置时，该角度将发生变化，这可能使其超出约束限制。

关于如何实现向后角度约束检查的任何想法？我不一定要寻找代码，而只是寻找如何做的理论或伪代码。我查看了GitHub上的几个不同的库，但无法追踪它们如何执行旋转约束。

注意：我知道我需要进行迭代，直到每次FABRIK计算都在某个距离之内为止，但我没有这样做，因为我目前可以多次单击同一点进行模拟每次迭代。但是，多次迭代不能解决我的问题。

FABRIK向后计算的旋转约束

0 个答案: