Инструментарий Helix, получение нового вектора вращения

У меня есть 6-осевой робот.

Я пытаюсь заставить каждую ось двигаться независимо. Все оси роботов перемещаются правильно, когда первая ось находится под исходным углом, но когда она была изменена, все остальные оси перемещаются неправильно. Они сохраняют правильную точку вращения, а не новый вектор.

Скошенная ось

Я следовал этому руководству http://www.barth-dev.de/getting-started-3d-wpf/

//moves axis_1
void move_axis_1(double angle)
{
    //rotate the object by "angle", the vector describes the axis
    RotateTransform3D axis_1_transform = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 0, 1), angle));
    //tells where the point of rotation is
    axis_1_transform.CenterX = 77.5;
    axis_1_transform.CenterY = 21.5;
    axis_1_transform.CenterZ = -8;
    //apply transformation
    axis_1.Transform = axis_1_transform;
    //also move the axis 2
    move_axis_2(axis_2_angle);            
}

//moves axis_2
void move_axis_2(double angle)
{
    //new group of transformations, the group will "add" movements
    var Group_3D = new Transform3DGroup();
    Group_3D.Children.Add(axis_1.Transform);

    //we need to find out where our old point is now
    Point3D origin = Group_3D.Transform(new Point3D(84.6, 21, -2));

    //create new transformation
    RotateTransform3D axis_2_transform = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 1, 0), angle));
    axis_2_transform.CenterX = origin.X;
    axis_2_transform.CenterY = origin.Y;
    axis_2_transform.CenterZ = origin.Z;

    //add it to the transformation group (and therefore to axis_1 movement
    Group_3D.Children.Add(axis_2_transform);

    //Apply the transform
    axis_2.Transform = Group_3D;

    //also move
    move_axis_3(axis_3_angle);         
}

//moves axis_3
void move_axis_3(double angle)
{
    //new group of transformations, the group will "add" movements
    var Group_3D = new Transform3DGroup();
    Group_3D.Children.Add(axis_2.Transform);

    //we need to find out where our old point is now
    Point3D origin = Group_3D.Transform(new Point3D(84.6, 17, 16.2));

    //create new transformation
    RotateTransform3D axis_3_transform = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 1, 0), angle));
    axis_3_transform.CenterX = origin.X;
    axis_3_transform.CenterY = origin.Y;
    axis_3_transform.CenterZ = origin.Z;

    //add it to the transformation group 
    Group_3D.Children.Add(axis_3_transform);

    //Apply the transform
    axis_3.Transform = Group_3D;

    //also move
    move_axis_4(axis_4_angle);           
}