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