void LateUpdate() { var meanHeight = (_motor1Joint2Tip.position.y + _motor2Joint2Tip.position.y) / 2f; transform.position = Vector3.up * meanHeight; var gamma = MiscMath.TiltFromOpposingPositions(_motor1Joint2Tip.position, _motor2Joint2Tip.position); var beta = MiscMath.TiltFromOpposingPositions(_motor3Joint2Tip.position, _motor4Joint2Tip.position); transform.localRotation = Quaternion.Euler(0f, -gamma * Mathf.Rad2Deg, 0f) * Quaternion.Euler(beta * Mathf.Rad2Deg, 0f, 0f); _upperMostRotationalJoint1.Rotation = -beta; _upperMostRotationalJoint2.Rotation = beta; _upperMostRotationalJoint3.Rotation = gamma; _upperMostRotationalJoint4.Rotation = -gamma; }
/// <summary> /// Translates a High Level Machine State into a Low Level Machine State /// </summary> public static LLMachineState Translate(this HLMachineState hlState) { // NOTE: We are adding the origin height because for all IK calculations we need the height relative to // motor shaft position and not just the height from origin/rest position of the plate. var ikHeight = hlState.Height + Constants.HeightOrigin; var xHeightOffset = MiscMath.HeightDifferenceFromTilt(hlState.XTilt); var yHeightOffset = MiscMath.HeightDifferenceFromTilt(hlState.YTilt); var xWidthOffset = MiscMath.WidthDifferenceFromTilt(hlState.XTilt); var yWidthOffset = MiscMath.WidthDifferenceFromTilt(hlState.YTilt); var m1Rot = ik.CalculateJoint1RotationFromTargetY(ikHeight + xHeightOffset, xWidthOffset); var m2Rot = ik.CalculateJoint1RotationFromTargetY(ikHeight - xHeightOffset, xWidthOffset); var m3Rot = ik.CalculateJoint1RotationFromTargetY(ikHeight + yHeightOffset, yWidthOffset); var m4Rot = ik.CalculateJoint1RotationFromTargetY(ikHeight - yHeightOffset, yWidthOffset); var m1J2Rot = ik.CalculateJoint2RotationFromJoint1Rotation(m1Rot, xWidthOffset); var m2J2Rot = ik.CalculateJoint2RotationFromJoint1Rotation(m2Rot, xWidthOffset); var m3J2Rot = ik.CalculateJoint2RotationFromJoint1Rotation(m3Rot, yWidthOffset); var m4J2Rot = ik.CalculateJoint2RotationFromJoint1Rotation(m4Rot, yWidthOffset); return(new LLMachineState(m1Rot, m2Rot, m3Rot, m4Rot, m1J2Rot, m2J2Rot, m3J2Rot, m4J2Rot)); }