예제 #1
0
    private void DoForwardKinematics()
    {
        UpdateThetaValues();
        circlesCreation.UpdateInvisibleCircles(thetas);

        endEffector.localPosition = circlesCreation.endEffectorPosition;

        legsRotation.RotateLegs(thetas, true);
        legsRotation.RotateParallelograms();
    }
예제 #2
0
    private void DoInverseKinematics()
    {
        Vector3 P = new Vector3(endEffector.localPosition.x, endEffector.localPosition.z, endEffector.localPosition.y);

        legsGenerator.GenerateEi(P);
        legsGenerator.GenerateFi(P);
        legsGenerator.GenerateGi(P);
        SetLegsGenerator();
        for (int i = 0; i < DeltaRobotIKUtils.NO_OF_LEGS; i++)
        {
            double currentFi = serviceConverter.ConvertToTwoDecimalPlaces(Fi[i]);
            double currentEi = serviceConverter.ConvertToTwoDecimalPlaces(Ei[i]);
            double currentGi = serviceConverter.ConvertToTwoDecimalPlaces(Gi[i]);
            double roots     = legsGenerator.GetRootsTi(currentFi, currentEi, currentGi);
            thetas[i] = 2 * Math.Atan(roots);
        }
        legsRotation.RotateLegs(thetas, false);
        legsRotation.RotateParallelograms();
    }