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