// Start is called before the first frame update
void Awake()
{
Robot = new RobotDynamics.Robots.Robot();
Robot
.AddJoint('y', new Vector(0, 0.03293461, 0))
.AddLinearJoint(new Vector(0, 0, 0), new Vector(0, 1, 0))
.AddJoint('y', new Vector(0.2241943, 0.4691764, 0))
.AddJoint('y', new Vector(0.1998537, -0.05284593, 0));
}
// Start is called before the first frame update
void Awake()
{
Robot = new RobotDynamics.Robots.Robot();
Robot
.AddJoint('y', new Vector(0, 0.03293461, 0))
.AddLinearJoint(new Vector(0, .5, 0), new Vector(0, 1, 0))
.AddJoint('y', new Vector(-0.2681684, 0.01463607, -0.0003781915))
.AddJoint('y', new Vector(0.1998537, -0.05284593, 0));
}
public void MixedRobot()
{
var Robot = new RobotDynamics.Robots.Robot();
Robot.AddJoint('x', new Vector(0, 4, 0))
.AddJoint('x', new Vector(0, 4, 0))
.AddLinearJoint(new Vector(0, 0, 0), new Vector(0, 0, 1));
Robot.ComputeForwardKinematics(new double[] { 0, 0, 0 });
}
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);
}