public static void solveKinematics(object parameter) { var kinematicProblem = (KinematicProblem)parameter; KinematicsSolver.InverseKinematics(kinematicProblem); _jointAngles = kinematicProblem.angles; _kinnematicsSolverFinished = true; _robotMovedFinished = false; }
private void UpdatePlacementPose() { var screenCenter = _camera.ViewportToScreenPoint(new Vector3(0.5f, 0.5f)); var screenCenter2D = new Vector2(screenCenter.x, screenCenter.y); if (!_arRaycastManager.Raycast(screenCenter2D, s_Hits, TrackableType.PlaneWithinPolygon)) { return; } // Raycast hits are sorted by distance, so the first one will be the closest hit. var hitPose = s_Hits[0].pose; var cameraForward = _camera.transform.forward; var cameraBearing = new Vector3(cameraForward.x, 0, cameraForward.z).normalized; var rotation = Quaternion.LookRotation(cameraBearing); placementCursor.transform.SetPositionAndRotation(hitPose.position, rotation); // maybe use identity rotation ? if (!(Input.touchCount > 0)) { return; } // If clicking on one of UI element, no raycast or robot interaction possible if (IsPointerOverUIObject()) { return; } spawnedRobot = Instantiate(m_PlacedPrefab, hitPose.position, rotation); _kinematicsSolver = spawnedRobot.GetComponentInChildren <KinematicsSolver>(); var joints = _kinematicsSolver.Joints; // _jointAngles = new float[joints.Length]; screenLogger.text = "robot pos:" + spawnedRobot.transform.position.ToString() + "\n"; placementCursor.SetActive(false); StartCoroutine(waitSeconds(1.0f)); }
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); } }