示例#1
0
    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);
        }