diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt
index 3324e15e23..f70e93ac0e 100644
--- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt
+++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt
@@ -16,10 +16,12 @@ class Constraint(
 	val constraintType: ConstraintType,
 	twist: Float = 0.0f,
 	swing: Float = 0.0f,
+	allowedDeviation: Float = 0f
 ) {
 	private val constraintFunction = constraintTypeToFunc(constraintType)
 	private val twistRad = Math.toRadians(twist.toDouble()).toFloat()
 	private val swingRad = Math.toRadians(swing.toDouble()).toFloat()
+	private val allowedDeviationRad = Math.toRadians(allowedDeviation.toDouble()).toFloat()
 	var hasTrackerRotation = false
 
 	/**
@@ -27,12 +29,26 @@ class Constraint(
 	 */
 	var allowModifications = true
 
+	var initialRotation = Quaternion.IDENTITY
+
 	/**
 	 * Apply rotational constraints and if applicable force the rotation
 	 * to be unchanged unless it violates the constraints
 	 */
 	fun applyConstraint(rotation: Quaternion, thisBone: Bone): Quaternion = constraintFunction(rotation, thisBone, swingRad, twistRad).unit()
 
+	/**
+	 * Force the given rotation to be within allowedDeviation degrees away from
+	 * initialRotation on both the twist and swing axis
+	 */
+	fun constrainToInitialRotation(rotation: Quaternion): Quaternion {
+		val rotationLocal = rotation * initialRotation.inv()
+		var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y)
+		swingQ = constrain(swingQ, allowedDeviationRad)
+		twistQ = constrain(twistQ, allowedDeviationRad)
+		return initialRotation * (swingQ * twistQ)
+	}
+
 	companion object {
 		enum class ConstraintType {
 			TWIST_SWING,
@@ -51,10 +67,10 @@ class Constraint(
 			rotation: Quaternion,
 			twistAxis: Vector3,
 		): Pair<Quaternion, Quaternion> {
-			val projection = rotation.project(twistAxis)
+			val projection = rotation.project(twistAxis).unit()
 
 			val twist = Quaternion(rotation.w, projection.xyz).unit()
-			val swing = rotation * twist.inv()
+			val swing = (rotation * twist.inv()).unit()
 
 			return Pair(swing, twist)
 		}
@@ -76,7 +92,7 @@ class Constraint(
 				)
 			}
 
-			return rot
+			return rot.unit()
 		}
 
 		private fun constrain(rotation: Quaternion, minAngle: Float, maxAngle: Float, axis: Vector3): Quaternion {
@@ -109,14 +125,15 @@ class Constraint(
 					rotation
 				} else {
 					val parent = thisBone.parent!!
+					val localRotationOffset = parent.rotationOffset.inv() * thisBone.rotationOffset
 					val rotationLocal =
-						(parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation
+						(parent.getGlobalRotation() * localRotationOffset).inv() * rotation
 					var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y)
 
 					swingQ = constrain(swingQ, swingRad)
 					twistQ = constrain(twistQ, twistRad)
 
-					parent.getGlobalRotation() * thisBone.rotationOffset * (swingQ * twistQ)
+					(parent.getGlobalRotation() * localRotationOffset * (swingQ * twistQ)).unit()
 				}
 			}
 
@@ -127,13 +144,15 @@ class Constraint(
 					rotation
 				} else {
 					val parent = thisBone.parent!!
+					val localRotationOffset = parent.rotationOffset.inv() * thisBone.rotationOffset
 					val rotationLocal =
-						(parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation
+						(parent.getGlobalRotation() * localRotationOffset).inv() * rotation
+
 					var (_, hingeAxisRot) = decompose(rotationLocal, Vector3.NEG_X)
 
 					hingeAxisRot = constrain(hingeAxisRot, min, max, Vector3.NEG_X)
 
-					parent.getGlobalRotation() * thisBone.rotationOffset * hingeAxisRot
+					(parent.getGlobalRotation() * localRotationOffset * hingeAxisRot).unit()
 				}
 			}
 
diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt
index 2bf811b894..9b2b76d10f 100644
--- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt
+++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt
@@ -37,32 +37,32 @@ class HumanSkeleton(
 	// Upper body bones
 	val headBone = Bone(BoneType.HEAD, Constraint(ConstraintType.COMPLETE))
 	val neckBone = Bone(BoneType.NECK, Constraint(ConstraintType.COMPLETE))
-	val upperChestBone = Bone(BoneType.UPPER_CHEST, Constraint(ConstraintType.TWIST_SWING, 10f, 80f))
-	val chestBone = Bone(BoneType.CHEST, Constraint(ConstraintType.TWIST_SWING, 10f, 80f))
-	val waistBone = Bone(BoneType.WAIST, Constraint(ConstraintType.TWIST_SWING, 10f, 80f))
-	val hipBone = Bone(BoneType.HIP, Constraint(ConstraintType.TWIST_SWING, 10f, 80f))
+	val upperChestBone = Bone(BoneType.UPPER_CHEST, Constraint(ConstraintType.TWIST_SWING, 15f, 80f))
+	val chestBone = Bone(BoneType.CHEST, Constraint(ConstraintType.TWIST_SWING, 15f, 80f))
+	val waistBone = Bone(BoneType.WAIST, Constraint(ConstraintType.TWIST_SWING, 15f, 80f))
+	val hipBone = Bone(BoneType.HIP, Constraint(ConstraintType.TWIST_SWING, 20f, 80f))
 
 	// Lower body bones
-	val leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(ConstraintType.TWIST_SWING, 180f, 15f))
-	val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(ConstraintType.TWIST_SWING, 180f, 15f))
-	val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 110f, 120f))
-	val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 110f, 120f))
+	val leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(ConstraintType.TWIST_SWING, 60f, 15f))
+	val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(ConstraintType.TWIST_SWING, 60f, 15f))
+	val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 90f, 170f))
+	val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 90f, 170f))
 	val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, Constraint(ConstraintType.HINGE, 180f, 0f))
 	val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, Constraint(ConstraintType.HINGE, 180f, 0f))
-	val leftFootBone = Bone(BoneType.LEFT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 90f))
-	val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 90f))
+	val leftFootBone = Bone(BoneType.LEFT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 50f))
+	val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 50f))
 
 	// Arm bones
 	val leftUpperShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(ConstraintType.COMPLETE))
 	val rightUpperShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.COMPLETE))
-	val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 180f, 10f))
-	val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 180f, 10f))
+	val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 20f, 10f))
+	val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 20f, 10f))
 	val leftUpperArmBone = Bone(BoneType.LEFT_UPPER_ARM, Constraint(ConstraintType.TWIST_SWING, 180f, 180f))
 	val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM, Constraint(ConstraintType.TWIST_SWING, 180f, 180f))
 	val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, Constraint(ConstraintType.HINGE, 0f, -180f))
 	val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, Constraint(ConstraintType.HINGE, 0f, -180f))
-	val leftHandBone = Bone(BoneType.LEFT_HAND, Constraint(ConstraintType.TWIST_SWING, 180f, 180f))
-	val rightHandBone = Bone(BoneType.RIGHT_HAND, Constraint(ConstraintType.TWIST_SWING, 180f, 180f))
+	val leftHandBone = Bone(BoneType.LEFT_HAND, Constraint(ConstraintType.TWIST_SWING, 90f, 90f))
+	val rightHandBone = Bone(BoneType.RIGHT_HAND, Constraint(ConstraintType.TWIST_SWING, 90f, 90f))
 
 	// Tracker bones
 	val headTrackerBone = Bone(BoneType.HEAD_TRACKER, Constraint(ConstraintType.COMPLETE))
@@ -1000,6 +1000,8 @@ class HumanSkeleton(
 			rightLowerLegBone,
 			leftFootBone,
 			rightFootBone,
+			leftUpperShoulderBone,
+			rightUpperShoulderBone,
 			leftShoulderBone,
 			rightShoulderBone,
 			leftUpperArmBone,
diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt
index 3ae847c52f..dff440f97c 100644
--- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt
+++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt
@@ -40,7 +40,7 @@ class IKChain(
 	 */
 	private fun prepBones() {
 		for (i in 0..<bones.size) {
-			if (!bones[i].rotationConstraint.hasTrackerRotation && bones[i].rotationConstraint.constraintType != ConstraintType.COMPLETE) {
+			if (bones[i].rotationConstraint.constraintType != ConstraintType.COMPLETE && bones[i].rotationConstraint.allowModifications) {
 				bones[i].setRotationRaw(rotations[i])
 			}
 		}
@@ -53,21 +53,18 @@ class IKChain(
 			val currentBone = bones[i]
 			val childBone = if (bones.size - 1 < i) bones[i + 1] else null
 
-			if (currentBone.boneType.bodyPart in IKSolver.LOCK_ROTATION) continue
+			if (currentBone.boneType.bodyPart in IKSolver.LOCK_ROTATION) {
+				rotations[i] = currentBone.getGlobalRotation()
+				continue
+			}
 
 			// Get the local position of the end effector and the target relative to the current node
 			val endEffectorLocal = (getEndEffectorsAvg() - currentBone.getPosition()).unit()
 			val targetLocal = (target - currentBone.getPosition()).unit()
 
 			// Compute the axis of rotation and angle for this bone
-			val cross = endEffectorLocal.cross(targetLocal).unit()
-			if (cross.lenSq() < 1e-9) continue
-			val baseAngle = acos(endEffectorLocal.dot(targetLocal).coerceIn(-1.0f, 1.0f))
-
-			val angle = baseAngle * IKSolver.DAMPENING_FACTOR * if (currentBone.rotationConstraint.allowModifications) 1f else IKSolver.STATIC_DAMPENING
-
-			val sinHalfAngle = sin(angle / 2)
-			val adjustment = Quaternion(cos(angle / 2), cross * sinHalfAngle)
+			val scalar = IKSolver.DAMPENING_FACTOR * if (currentBone.rotationConstraint.allowModifications) 1f else IKSolver.STATIC_DAMPENING
+			val adjustment = Quaternion.fromTo(endEffectorLocal, targetLocal).pow(scalar).unit()
 			val correctedRot = (adjustment * currentBone.getGlobalRotation()).unit()
 
 			rotations[i] = setBoneRotation(currentBone, childBone, correctedRot, useConstraints)
@@ -101,7 +98,11 @@ class IKChain(
 	 */
 	fun resetChain() {
 		distToTargetSqr = Float.POSITIVE_INFINITY
-		// prepBones()
+
+		for (b in bones) {
+			b.rotationConstraint.initialRotation = b.getGlobalRotation()
+		}
+		//prepBones()
 
 		for (child in children) {
 			child.resetChain()
@@ -136,7 +137,9 @@ class IKChain(
 	 */
 	private fun setBoneRotation(bone: Bone, childBone: Bone?, rotation: Quaternion, useConstraints: Boolean): Quaternion {
 		// Constrain relative to the parent
-		val newRotation = if ((useConstraints || bone.rotationConstraint.constraintType == ConstraintType.COMPLETE) && !bone.rotationConstraint.hasTrackerRotation) {
+		val newRotation = if (bone.rotationConstraint.constraintType == ConstraintType.COMPLETE) {
+			bone.rotationConstraint.applyConstraint(rotation, bone)
+		} else if (useConstraints && !bone.rotationConstraint.hasTrackerRotation) {
 			bone.rotationConstraint.applyConstraint(rotation, bone)
 		} else {
 			rotation
diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt
index 84f1ab1468..d2d5dbeb83 100644
--- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt
+++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt
@@ -13,7 +13,6 @@ import solarxr_protocol.datatypes.BodyPart
 class IKSolver(private val root: Bone) {
 	companion object {
 		const val TOLERANCE_SQR = 1e-8 // == 0.01 cm
-		const val ROTATIONAL_TOLERANCE = 0.000745329 // == 0.5 degrees
 		const val MAX_ITERATIONS = 100
 		const val ANNEALING_STEP = 20
 		const val ANNEALING_ITERATIONS = 5
@@ -223,6 +222,7 @@ class IKSolver(private val root: Bone) {
 				}
 			}
 
+			// Terminate if using constraints and the chain is solved
 			if (solved && useConstraints) return true
 		}