diff --git a/Content/Blueprints/BP_Puma.uasset b/Content/Blueprints/BP_Puma.uasset index 48c6e2a..a726691 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 268db87..998dcf1 100644 Binary files a/Content/Blueprints/BP_TestBot.uasset and b/Content/Blueprints/BP_TestBot.uasset differ diff --git a/Content/Levels/Main.umap b/Content/Levels/Main.umap index 00d1eb6..27accfe 100644 Binary files a/Content/Levels/Main.umap and b/Content/Levels/Main.umap differ diff --git a/Content/Models/puma_link4.uasset b/Content/Models/puma_link4.uasset index 1ad3291..2491f0a 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 0a8ebc0..fd574ae 100644 Binary files a/Content/Models/puma_link5.uasset and b/Content/Models/puma_link5.uasset differ diff --git a/Content/Models/puma_link6.uasset b/Content/Models/puma_link6.uasset index 682bfb3..61e8274 100644 Binary files a/Content/Models/puma_link6.uasset and b/Content/Models/puma_link6.uasset differ diff --git a/Content/Models/puma_link7.uasset b/Content/Models/puma_link7.uasset index 301038d..339b829 100644 Binary files a/Content/Models/puma_link7.uasset and b/Content/Models/puma_link7.uasset differ diff --git a/Source/PBDRobotics/Joint.cpp b/Source/PBDRobotics/Joint.cpp index a972715..b1cf6c4 100644 --- a/Source/PBDRobotics/Joint.cpp +++ b/Source/PBDRobotics/Joint.cpp @@ -15,36 +15,73 @@ Vector3d UJoint::GetSecondWorldPosition() const{ return SecondLink->Position + SecondLink->Orientation * SecondLocalPosition; } +Vector3d UJoint::GetFirstWorldAxis() const{ + return FirstLink->Orientation * FirstRotateAxis; +} +Vector3d UJoint::GetSecondWorldAxis() const{ + return SecondLink->Orientation * SecondRotateAxis; +} + void UJoint::SolvePosition(const double H) const{ ULink* L1 = FirstLink; ULink* L2 = SecondLink; // Positional Constraints - const Vector3d R1 = GetFirstWorldPosition(); - const Vector3d R2 = GetSecondWorldPosition(); - const Vector3d D = R1 - R2; // Original PBD (not R2 - R1), because Impulse P is negated - const Vector3d N = D.normalized(); - const double C = D.norm(); - const Matrix3d I1 = L1->GetInertia(); - const Matrix3d I2 = L2->GetInertia(); - const double M1 = L1->Mass; - const double M2 = L2->Mass; + { + const Vector3d R1 = GetFirstWorldPosition(); + const Vector3d R2 = GetSecondWorldPosition(); + const Vector3d D = R1 - R2; // Original PBD (not R2 - R1), because Impulse P is negated + const Vector3d N = D.normalized(); + const double C = D.norm(); + const Matrix3d I1 = L1->GetInertia(); + const Matrix3d I2 = L2->GetInertia(); + const double M1 = L1->Mass; + const double M2 = L2->Mass; - const double W1 = L1->GetPositionalInverseMass(R1, N); - const double W2 = L2->GetPositionalInverseMass(R2, N); + const double W1 = L1->GetPositionalInverseMass(R1, N); + const double W2 = L2->GetPositionalInverseMass(R2, N); - const double A = 0.000001 / (H * H); - const Vector3d P = -C / (W1 + W2 + A) * N; + constexpr double A = 1. / 10000000; // Compliance (inverse of stiffness) + const Vector3d P = -C / (W1 + W2 + A / (H * H)) * N; - L1->Position += P / M1; - L2->Position -= P / M2; + L1->Position += P / M1; + L2->Position -= P / M2; - Vector3d T1 = I1.inverse() * R1.cross(P); - Vector3d T2 = I2.inverse() * R2.cross(P); - L1->Orientation += Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5; - L2->Orientation -= Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5; - + Vector3d T1 = I1.inverse() * R1.cross(P); + Vector3d T2 = I2.inverse() * R2.cross(P); + Quaterniond Add1 = Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5; + Quaterniond Add2 = Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5; + //L1->Orientation = L1->Orientation + Add1; + //L2->Orientation = L2->Orientation- Add2; + //UE_LOG(LogTemp, Log, TEXT("%f %f"), (T1).norm(), T2.norm()); + } + // Rotational Constraints + { + const Vector3d A1 = GetFirstWorldAxis(); + const Vector3d A2 = GetSecondWorldAxis(); + const Vector3d Q = A1.cross(A2); + const Vector3d N = Q.normalized(); + const double Theta = Q.norm(); + + const Matrix3d I1 = L1->GetInertia(); + const Matrix3d I2 = L2->GetInertia(); + + const double W1 = L1->GetRotationalInverseMass(N); + const double W2 = L2->GetRotationalInverseMass(N); + + const double A = 0.000001 / (H * H); + const Vector3d P = -Theta / (W1 + W2 + A) * N; + + Vector3d T1 = I1.inverse() * P; + Vector3d T2 = I2.inverse() * P; + T1 = T2 = P; + //L1->Orientation += Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5; + //L2->Orientation -= Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5; + + // UE_LOG(LogTemp, Log, TEXT("%f | %f"), W1, W2); + } + } void UJoint::SolveVelocity(const double H) const{ @@ -54,8 +91,8 @@ void UJoint::SolveVelocity(const double H) const{ // Damping const Vector3d V1 = L1->Velocity; const Vector3d V2 = L2->Velocity; - - constexpr double MuLin = 100000; + + constexpr double MuLin = 100000; // Damping strength const Vector3d DeltaV = (V2 - V1) * UKismetMathLibrary::Min(MuLin * H, 1); const Vector3d R1 = GetFirstWorldPosition(); @@ -74,8 +111,8 @@ void UJoint::SolveVelocity(const double H) const{ L1->Velocity += P / L1->Mass; L2->Velocity -= P / L2->Mass; - L1->AngularVelocity += I1.inverse() * R1.cross(P); - L2->AngularVelocity -= I2.inverse() * R2.cross(P); + //L1->AngularVelocity += I1.inverse() * R1.cross(P); + //L2->AngularVelocity -= I2.inverse() * R2.cross(P); } UJoint::UJoint() diff --git a/Source/PBDRobotics/Joint.h b/Source/PBDRobotics/Joint.h index fc4c7dd..d4ec465 100644 --- a/Source/PBDRobotics/Joint.h +++ b/Source/PBDRobotics/Joint.h @@ -24,15 +24,18 @@ public: UPROPERTY(BlueprintReadWrite) ULink* SecondLink; - Vector3d FirstRotateAxis; - Vector3d SecondRotateAxis; - Vector3d FirstLocalPosition; Vector3d SecondLocalPosition; Vector3d GetFirstWorldPosition() const; Vector3d GetSecondWorldPosition() const; + Vector3d FirstRotateAxis; + Vector3d SecondRotateAxis; + + Vector3d GetFirstWorldAxis() const; + Vector3d GetSecondWorldAxis() const; + void SolvePosition(const double H) const; void SolveVelocity(const double H) const; diff --git a/Source/PBDRobotics/Link.cpp b/Source/PBDRobotics/Link.cpp index 5c60304..b84d99e 100644 --- a/Source/PBDRobotics/Link.cpp +++ b/Source/PBDRobotics/Link.cpp @@ -20,8 +20,9 @@ double ULink::GetPositionalInverseMass(const Vector3d R, const Vector3d N) const const Matrix3d I = GetInertia(); return 1 / M + R.cross(N).transpose() * I.inverse() * R.cross(N); } -double ULink::GetRotationalInverseMass() const{ - return 0; +double ULink::GetRotationalInverseMass(const Vector3d N) const{ + const Matrix3d I = GetInertia(); + return N.transpose() * I.inverse() * N; } void ULink::Setup(){ @@ -33,9 +34,22 @@ void ULink::SetupProperties(){ Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal(); Position = ToEigen(GetComponentLocation()); Velocity = Vector3d::Zero(); - Mass = IsBase ? INFINITY : CalculateMass(); - Orientation = Quaterniond::Identity(); + Mass = CalculateMass(); + Orientation = ToEigen(GetComponentQuat()); AngularVelocity = Vector3d::Zero(); + + if (IsBase){ + Mass = 1e30; + Inertia_Tensor_Local = Vector3d::Ones().asDiagonal() * 1e30; + } + + const Matrix3d I = GetInertia().inverse(); + UE_LOG(LogTemp, Log, TEXT("%s"), *GetName()); + auto log = [this, I](int col){ + UE_LOG(LogTemp, Log, TEXT("%f %f %f"), I(col, 0), I(col, 1), I(col, 2)); + }; + log(0);log(1);log(2); + } void ULink::SetupJoints() const { @@ -54,7 +68,9 @@ void ULink::SetupJoints() const { } void ULink::Update(const double H){ - const Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 0; + Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 0; + if (IsBase) + ExtTransForce = Vector3d::Zero(); Last_Position = Position; Velocity += H * ExtTransForce / Mass; @@ -63,7 +79,7 @@ void ULink::Update(const double H){ Last_Orientation = Orientation; // Angle Velocity has no external contributor Vector3d w = AngularVelocity; - Orientation += Quaterniond(0, w.x(), w.y(), w.z()) * Orientation * 0.5 * H; + Orientation = Orientation + Quaterniond(0, w.x(), w.y(), w.z()) * Orientation * 0.5 * H; Orientation.normalize(); } diff --git a/Source/PBDRobotics/Link.h b/Source/PBDRobotics/Link.h index a4f0f18..82eb4b8 100644 --- a/Source/PBDRobotics/Link.h +++ b/Source/PBDRobotics/Link.h @@ -44,7 +44,7 @@ public: Matrix3d GetInertia() const; double GetPositionalInverseMass(const Vector3d R, const Vector3d N) const; - double GetRotationalInverseMass() const; + double GetRotationalInverseMass(const Vector3d N) const; void Setup(); private: diff --git a/Source/PBDRobotics/Robot.cpp b/Source/PBDRobotics/Robot.cpp index 1d878b6..b0aad8c 100644 --- a/Source/PBDRobotics/Robot.cpp +++ b/Source/PBDRobotics/Robot.cpp @@ -25,7 +25,7 @@ void ARobot::BeginPlay(){ void ARobot::Tick(const float DeltaTime){ Super::Tick(DeltaTime); - constexpr int SubSteps = 100; + constexpr int SubSteps = 30; const double h = DeltaTime / SubSteps; for (int i = 0; i < SubSteps; i++){ for (auto* Link : Parts) diff --git a/Source/PBDRobotics/util.h b/Source/PBDRobotics/util.h index b5707a9..9b4f420 100644 --- a/Source/PBDRobotics/util.h +++ b/Source/PBDRobotics/util.h @@ -6,6 +6,10 @@ inline Vector3d ToEigen(const FVector V){ return Vector3d(V.X, V.Y, V.Z); } +inline Quaterniond ToEigen(const FQuat Q){ + return Quaterniond(Q.W, Q.X, Q.Y, Q.Z); +} + inline FVector FromEigen(const Vector3d V){ return FVector(V.x(), V.y(), V.z()); } @@ -22,10 +26,6 @@ inline Quaterniond operator+ (const Quaterniond Q1, const Quaterniond Q2){ return Quaterniond(Q1.w() + Q2.w(), Q1.x() + Q2.x(), Q1.y() + Q2.y(), Q1.z() + Q2.z()); } -inline Quaterniond operator+= (const Quaterniond Q1, const Quaterniond Q2){ - return Q1 + Q2; -} - inline Quaterniond operator- (const Quaterniond Q1, const Quaterniond Q2){ return Quaterniond(Q1.w() - Q2.w(), Q1.x() - Q2.x(), Q1.y() - Q2.y(), Q1.z() - Q2.z()); }