protected void SetQ(double[] q)
{
var transformations = Robot.ComputeForwardKinematics(q);
for (int i = 0; i < transformations.Count - 1; i++)
{
Joints[i].SetJointValue(transformations[i], transformations[i + 1], Robot.Links[i], q[i]);
}
}
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 });
}