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