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