diff --git a/Content/Blueprints/BP_Puma.uasset b/Content/Blueprints/BP_Puma.uasset
index a726691..f764713 100644
Binary files a/Content/Blueprints/BP_Puma.uasset and b/Content/Blueprints/BP_Puma.uasset differ
diff --git a/Content/Blueprints/BP_TestBot.uasset b/Content/Blueprints/BP_TestBot.uasset
index cc4ac29..4e0f8da 100644
Binary files a/Content/Blueprints/BP_TestBot.uasset and b/Content/Blueprints/BP_TestBot.uasset differ
diff --git a/Content/Blueprints/BP_TestCube.uasset b/Content/Blueprints/BP_TestCube.uasset
index 74dab91..815eba9 100644
Binary files a/Content/Blueprints/BP_TestCube.uasset and b/Content/Blueprints/BP_TestCube.uasset differ
diff --git a/Content/Levels/Main.umap b/Content/Levels/Main.umap
index 528b448..b6dd1d4 100644
Binary files a/Content/Levels/Main.umap and b/Content/Levels/Main.umap differ
diff --git a/Content/Models/puma_link1.uasset b/Content/Models/puma_link1.uasset
index 9ec1a03..7e578cf 100644
Binary files a/Content/Models/puma_link1.uasset and b/Content/Models/puma_link1.uasset differ
diff --git a/Content/Models/puma_link2.uasset b/Content/Models/puma_link2.uasset
index 5c3fc24..c639db0 100644
Binary files a/Content/Models/puma_link2.uasset and b/Content/Models/puma_link2.uasset differ
diff --git a/Content/Models/puma_link3.uasset b/Content/Models/puma_link3.uasset
index 1164e60..529fd0a 100644
Binary files a/Content/Models/puma_link3.uasset and b/Content/Models/puma_link3.uasset differ
diff --git a/Content/Models/puma_link4.uasset b/Content/Models/puma_link4.uasset
index 2491f0a..b61764d 100644
Binary files a/Content/Models/puma_link4.uasset and b/Content/Models/puma_link4.uasset differ
diff --git a/Content/Models/puma_link5.uasset b/Content/Models/puma_link5.uasset
index fd574ae..76bdb6d 100644
Binary files a/Content/Models/puma_link5.uasset and b/Content/Models/puma_link5.uasset differ
diff --git a/Content/Models/puma_link7.uasset b/Content/Models/puma_link7.uasset
index 339b829..9d608ca 100644
Binary files a/Content/Models/puma_link7.uasset and b/Content/Models/puma_link7.uasset differ
diff --git a/Content/Models/robot.uasset b/Content/Models/robot.uasset
index fbcd735..21cdefe 100644
Binary files a/Content/Models/robot.uasset and b/Content/Models/robot.uasset differ
diff --git a/Content/Models/test_arm.uasset b/Content/Models/test_arm.uasset
deleted file mode 100644
index efcf86d..0000000
Binary files a/Content/Models/test_arm.uasset and /dev/null differ
diff --git a/Content/Models/test_arm_onejoint.uasset b/Content/Models/test_arm_onejoint.uasset
new file mode 100644
index 0000000..047200b
Binary files /dev/null and b/Content/Models/test_arm_onejoint.uasset differ
diff --git a/Content/Models/test_arm_twojoints.uasset b/Content/Models/test_arm_twojoints.uasset
new file mode 100644
index 0000000..d640598
Binary files /dev/null and b/Content/Models/test_arm_twojoints.uasset differ
diff --git a/Source/PBDRobotics/Joint.cpp b/Source/PBDRobotics/Joint.cpp
index 0a49084..2598100 100644
--- a/Source/PBDRobotics/Joint.cpp
+++ b/Source/PBDRobotics/Joint.cpp
@@ -48,9 +48,13 @@ void UJoint::SolvePosition(const double H) const{
 		const double W1 = L1->GetPositionalInverseMass(R1, N);
 		const double W2 = L2->GetPositionalInverseMass(R2, N);
 
-		constexpr double A = 0; //1. / 10000000; // Compliance (inverse of stiffness)
-		
-		const Vector3d P = -C / (W1 + W2 + A / (H * H)) * N;
+		constexpr double A = 0; //1. / 100000000; // Compliance (inverse of stiffness)
+		const double Denominator = W1 + W2 + A / (H * H);
+		Vector3d P;
+		if (Denominator == 0)
+			P = Vector3d::Zero();
+		else
+			P = N * -C / Denominator;
 	
 		L1->Position += P / M1;
 		L2->Position -= P / M2;
@@ -61,15 +65,20 @@ void UJoint::SolvePosition(const double H) const{
 		const Quaterniond Add2 = Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;
 		L1->Orientation = (L1->Orientation + Add1).normalized();
 		L2->Orientation = (L2->Orientation - Add2).normalized();
-		// UE_LOG(LogTemp, Log, TEXT("%f"), P.norm());
 	}
 	
 	// Rotational Constraints
 	{
-		const Vector3d A1 = GetFirstWorldAxis();
-		const Vector3d A2 = GetSecondWorldAxis();
-		assert(A1.norm() == A2.norm() == 1);
-		const Vector3d Q = A1.cross(A2);
+		Vector3d Q;
+		if (L2->IsEffector){
+			const Quaterniond Tq = L1->Orientation * L2->Orientation.inverse();
+			Q = 2 * Tq.vec();
+		} else{
+			const Vector3d A1 = GetFirstWorldAxis();
+			const Vector3d A2 = GetSecondWorldAxis();
+			assert(A1.norm() == A2.norm() == 1);
+			Q = A1.cross(A2);
+		}
 		const Vector3d N = Q.normalized();
 		const double Theta = UKismetMathLibrary::Asin(Q.norm());// * 180. / PI;
 
@@ -80,7 +89,12 @@ void UJoint::SolvePosition(const double H) const{
 		const double W2 = L2->GetRotationalInverseMass(N);
 
 		constexpr double A = 0; //1. / 100000000;
-		const Vector3d P = Theta / (W1 + W2 + A / (H * H)) * N;
+		const double Denominator = W1 + W2 + A / (H * H);
+		Vector3d P;
+		if (Denominator == 0)
+			P = Vector3d::Zero();
+		else
+			P = N * Theta / Denominator;
 		
 		Vector3d T1 = I1.inverse() * P;
 		Vector3d T2 = I2.inverse() * P;
@@ -88,7 +102,6 @@ void UJoint::SolvePosition(const double H) const{
 		const Quaterniond Add2 = Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;
 		L1->Orientation = (L1->Orientation + Add1).normalized();
 		L2->Orientation = (L2->Orientation - Add2).normalized();
-		// UE_LOG(LogTemp, Log, TEXT("%f"), Add2.norm());
 	}
 	
 }
diff --git a/Source/PBDRobotics/Link.h b/Source/PBDRobotics/Link.h
index 82eb4b8..ee886bf 100644
--- a/Source/PBDRobotics/Link.h
+++ b/Source/PBDRobotics/Link.h
@@ -47,6 +47,9 @@ public:
 	double GetRotationalInverseMass(const Vector3d N) const;
 	
 	void Setup();
+
+	UPROPERTY(EditAnywhere)
+	bool IsEffector = false;
 private:
 	UPROPERTY(EditAnywhere)
 	bool IsBase = false;