public void FollowTargetOneStep(GameObject Target) { if (EnableInverseKinematics) { if (Target.transform.localPosition != lastPos || Target.transform.localRotation != lastRot) { lastPos = Target.transform.localPosition; lastRot = Target.transform.localRotation; //Inverse Kinematics Vector r_des = Target.transform.localPosition.ToVector(); RotationMatrix C_des = Target.transform.EulerAnglesToRotationMatrix(); var result = Robot.ComputeInverseKinematics(r_des, C_des, alpha, Lambda, MaxIter, Tolerance, UseLastQAsInit ? last_q : null); if (result.DidConverge) { last_q = result.q; if (!EnablePController) { SetQ(result.q); } } } } }
public void LinearRobot() { var Robot = new RobotDynamics.Robots.Robot(); Robot.AddLinearJoint(new Vector(0, 0, 0), new Vector(1, 0, 0)) .AddLinearJoint(new Vector(0, 0, 0), new Vector(0, 1, 0)) .AddLinearJoint(new Vector(0, 0, 0), new Vector(0, 0, 1)); var result = Robot.ComputeInverseKinematics(new Vector(0, 0, 0), new RotationMatrix(Matrix.Eye(3).matrix)); Assert.IsTrue(result.DidConverge); Assert.IsTrue(result.q.All(x => x < 0.001f)); }
public void LinearRobot_3() { var Robot = new RobotDynamics.Robots.Robot(); Robot.AddLinearJoint(new Vector(0, 0, 0), new Vector(1, 0, 0)) .AddLinearJoint(new Vector(0, 0, 0), new Vector(0, 1, 0)) .AddLinearJoint(new Vector(0, 0, 0), new Vector(0, 0, 1)); var result = Robot.ComputeInverseKinematics(new Vector(1, -1, 1), new RotationMatrix(Matrix.Eye(3).matrix), 0.001, 0.05, 100, 0.001f); Assert.IsTrue(result.DidConverge); Assert.IsTrue(Math.Abs(result.q[0] - 1) < 0.01f); Assert.IsTrue(Math.Abs(result.q[1] + 1) < 0.01f); Assert.IsTrue(Math.Abs(result.q[2] - 1) < 0.01f); }