Exemplo n.º 1
0
    private void SolveInverseKinematics(Vector3 targetPosition)
    {
        var robotJoints  = _kinematicsSolver.Joints;
        var jointOffsets = robotJoints.Select(x => x.StartOffset).ToArray();
        var jointAxis    = robotJoints.Select(x => x.Axis).ToArray();

        float[] jointAngles = new float[_kinematicsSolver.Joints.Length];

        _kinematicProblem = new KinematicProblem(robotJoints[0].transform.position,
                                                 jointOffsets, jointAxis, targetPosition, jointAngles);
        _kinnematicsSolverFinished = false;
        Thread kinematicThread = new Thread(solveKinematics);

        kinematicThread.Start(_kinematicProblem);
    }
    public static Vector3 ForwardKinematics(KinematicProblem problem)
    {
        Vector3    prevPoint = problem.firstJointPosition;
        Quaternion rotation  = Quaternion.identity;

        for (int i = 1; i < problem.angles.Length; i++)
        {
            // Rotates around a new axis
            rotation *= Quaternion.AngleAxis(problem.angles[i - 1], problem.jointAxis[i - 1]);
            Vector3 nextPoint = prevPoint + rotation * problem.jointOffsets[i];

            prevPoint = nextPoint;
        }

        return(prevPoint);
    }
    public static float PartialGradient(KinematicProblem problem, int i)
    {
        // Saves the angle,
        // it will be restored later
        float angle = problem.angles[i];

        // Gradient : [F(x+SamplingDistance) - F(x)] / h
        float f_x = DistanceFromTarget(problem);

        problem.angles[i] += SamplingDistance;
        float f_x_plus_d = DistanceFromTarget(problem);

        float gradient = (f_x_plus_d - f_x) / SamplingDistance;

        // Restores
        problem.angles[i] = angle;

        return(gradient);
    }
    public static void InverseKinematics(KinematicProblem problem)
    {
        Debug.Log("------  Begin IK  ------");
        Debug.Log("Original distance: " + DistanceFromTarget(problem));
        Debug.Log("------------------------");

        var step = 0;

        // var maxSteps = 50000;

        while (step < maxSteps && !(DistanceFromTarget(problem) < DistanceThreshold))
        {
            for (int i = problem.angles.Length - 3; i >= 0; i--)
            {
                // Gradient descent
                // Update : Solution -= LearningRate * Gradient
                float gradient = PartialGradient(problem, i);
                problem.angles[i] -= LearningRate * gradient;

                // Early termination
                if (DistanceFromTarget(problem) < DistanceThreshold)
                {
                    Debug.Log("------ Early End IK  ------");
                    Debug.Log("Final distance (success true): " + DistanceFromTarget(problem).ToString("F4"));
                    return;
                }
            }

            step++;
        }

        var finalDistance = DistanceFromTarget(problem);

        Debug.Log("------  End IK  ------");
        Debug.Log("Final distance (success " + (finalDistance < DistanceThreshold) + "): " + finalDistance.ToString("F4"));
    }
Exemplo n.º 5
0
    private void interactWithLoadedRobot()
    {
        if (Input.touchCount > 0)
        {
            // If clicking on one of UI element, no raycast or robot interaction possible
            if (IsPointerOverUIObject())
            {
                return;
            }

            var screenTouchPosition = new Vector2(Input.GetTouch(0).position.x, Input.GetTouch(0).position.y);

            if (!_arRaycastManager.Raycast(screenTouchPosition, s_Hits, TrackableType.PlaneWithinPolygon))
            {
                return;
            }

            var hitPose = s_Hits[0].pose;

            _positionMarker = Instantiate(positionPrefab, hitPose.position, Quaternion.identity);

            StartCoroutine(waitSeconds(1.0f));

            UpdateJointsToPosition(hitPose.position);
        }

        if (!_robotMovedFinished)
        {
            var robotJoints          = _kinematicsSolver.Joints;
            int numberOfMovableJoint = robotJoints.Length - 2;

            var jointOffsets = robotJoints.Select(x => x.StartOffset).ToArray();
            var jointAxis    = new Vector3[]
            {
                new Vector3(1, 0, 0),
                new Vector3(0, 0, 1),
                new Vector3(0, 0, 1),
                new Vector3(1, 0, 0),
                new Vector3(0, 0, 1),
                new Vector3(1, 0, 0),
                new Vector3(0, 0, 0)
            };

            float[] jointAngles = new float[_kinematicsSolver.Joints.Length];

            var pb = new KinematicProblem(robotJoints[0].transform.position,
                                          jointOffsets, jointAxis, _kinematicProblem.targetPosition, jointAngles);

            Vector3 point = KinematicsSolver.ForwardKinematics(pb);

            screenLogger.text  = "Target: " + _kinematicProblem.targetPosition.ToString() + "\n";
            screenLogger.text += "Curr: " + point.ToString() + "\n";

            var distanceFromTarget = KinematicsSolver.DistanceFromTarget(pb);
            screenLogger.text += "Dist: " + distanceFromTarget + "\n";

            Dictionary <int, Vector3> _jointEditorAxis = new Dictionary <int, Vector3>(numberOfMovableJoint);
            _jointEditorAxis.Add(0, new Vector3(1, 0, 0));
            _jointEditorAxis.Add(1, new Vector3(0, 0, 1));
            _jointEditorAxis.Add(2, new Vector3(0, 0, 1));
            _jointEditorAxis.Add(3, new Vector3(1, 0, 0));
            _jointEditorAxis.Add(4, new Vector3(0, 0, 1));

            printArray(_jointAngles);

            float[] currAngles = new float[numberOfMovableJoint];

            for (int i = 0; i < numberOfMovableJoint; i++)
            {
                robotJoints[i].transform.rotation.ToAngleAxis(out float angle, out Vector3 axis);
                currAngles[i] = angle;
                var angleIncrement = _jointAngles[i] - angle;
                robotJoints[i].transform.Rotate(_jointEditorAxis[i], _jointAngles[i]);
            }

            printArray(currAngles);

            float[] updatedAngles = new float[numberOfMovableJoint];
            for (int i = 0; i < numberOfMovableJoint; i++)
            {
                robotJoints[i].transform.rotation.ToAngleAxis(out float angle, out Vector3 axis);
                updatedAngles[i] = angle;
            }

            pb = new KinematicProblem(robotJoints[0].transform.position,
                                      jointOffsets, jointAxis, _kinematicProblem.targetPosition, jointAngles);

            point = KinematicsSolver.ForwardKinematics(pb);

            printArray(updatedAngles);

            screenLogger.text += "Updated pos: " + point.ToString() + "\n";

            _robotMovedFinished = true;
            Destroy(_positionMarker);
        }
    }
    public static float DistanceFromTarget(KinematicProblem problem)
    {
        Vector3 point = ForwardKinematics(problem);

        return(Vector3.Distance(point, problem.targetPosition));
    }