PUMA with gravity works

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)
+		constexpr double A = 0; //1. / 100000000; // Compliance (inverse of stiffness)
-		
+		const double Denominator = W1 + W2 + A / (H * H);
-		const Vector3d P = -C / (W1 + W2 + A / (H * H)) * N;
+		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();
+		Vector3d Q;
-		const Vector3d A2 = GetSecondWorldAxis();
+		if (L2->IsEffector){
-		assert(A1.norm() == A2.norm() == 1);
+			const Quaterniond Tq = L1->Orientation * L2->Orientation.inverse();
-		const Vector3d Q = A1.cross(A2);
+			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());
 	}
 	
 }

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;