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")); }
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)); }