diff --git a/Config/DefaultEngine.ini b/Config/DefaultEngine.ini index e3068fc..40d5e6d 100644 --- a/Config/DefaultEngine.ini +++ b/Config/DefaultEngine.ini @@ -19,3 +19,8 @@ AppliedDefaultGraphicsPerformance=Maximum [/Script/Engine.RendererSettings] r.DefaultFeature.AutoExposure=False + +[CoreRedirects] ++ClassRedirects=(OldName="/Script/PBDRobotics.Joint",NewName="/Script/PBDRobotics.Joint") ++PropertyRedirects=(OldName="/Script/PBDRobotics.Joint.PrevLink",NewName="/Script/PBDRobotics.Joint.FirstLink") ++PropertyRedirects=(OldName="/Script/PBDRobotics.Joint.NextLink",NewName="/Script/PBDRobotics.Joint.SecondLink") \ No newline at end of file diff --git a/Content/Blueprints/BP_Puma.uasset b/Content/Blueprints/BP_Puma.uasset index db3d858..48c6e2a 100644 Binary files a/Content/Blueprints/BP_Puma.uasset and b/Content/Blueprints/BP_Puma.uasset differ diff --git a/Content/Blueprints/BP_Robot.uasset b/Content/Blueprints/BP_Robot.uasset index b03c769..b878317 100644 Binary files a/Content/Blueprints/BP_Robot.uasset and b/Content/Blueprints/BP_Robot.uasset differ diff --git a/Content/Blueprints/BP_TestBot.uasset b/Content/Blueprints/BP_TestBot.uasset new file mode 100644 index 0000000..397d475 Binary files /dev/null and b/Content/Blueprints/BP_TestBot.uasset differ diff --git a/Content/Levels/Main.umap b/Content/Levels/Main.umap index e0733bb..c4e4769 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 5f6efbd..9ec1a03 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 2d87875..5c3fc24 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 80d4656..b10bb7c 100644 Binary files a/Content/Models/puma_link3.uasset and b/Content/Models/puma_link3.uasset differ diff --git a/Source/PBDRobotics/Joint.cpp b/Source/PBDRobotics/Joint.cpp new file mode 100644 index 0000000..2e969ea --- /dev/null +++ b/Source/PBDRobotics/Joint.cpp @@ -0,0 +1,8 @@ +// Fill out your copyright notice in the Description page of Project Settings. + + +#include "Joint.h" + +UJoint::UJoint() +{ +} diff --git a/Source/PBDRobotics/Joint.h b/Source/PBDRobotics/Joint.h new file mode 100644 index 0000000..0a4eafb --- /dev/null +++ b/Source/PBDRobotics/Joint.h @@ -0,0 +1,34 @@ +// Fill out your copyright notice in the Description page of Project Settings. + +#pragma once + +#include "Eigen/Dense" +#include "CoreMinimal.h" +#include "Joint.generated.h" + +using namespace Eigen; + +class ULink; +/** + * + */ +UCLASS(BlueprintType, ClassGroup=(Custom)) +class PBDROBOTICS_API UJoint : public UObject +{ + GENERATED_BODY() +public: + + UPROPERTY(BlueprintReadWrite) + ULink* FirstLink; + + UPROPERTY(BlueprintReadWrite) + ULink* SecondLink; + + Vector3d FirstRotateAxis; + Vector3d SecondRotateAxis; + + Vector3d FirstLocalPosition; + Vector3d SecondLocalPosition; + + UJoint(); +}; diff --git a/Source/PBDRobotics/Link.cpp b/Source/PBDRobotics/Link.cpp index 73f17d3..41465a9 100644 --- a/Source/PBDRobotics/Link.cpp +++ b/Source/PBDRobotics/Link.cpp @@ -5,39 +5,77 @@ #include +#include "util.h" #include "Eigen/Dense" +#include "Engine/StaticMeshSocket.h" -Eigen::Matrix3d ULink::GetMomentOfInertia() const { - const FVector FI = this->GetInertiaTensor(); +using namespace Eigen; - // Inertial tensor of rotation - const Eigen::Vector3d I(FI.X, FI.Y, FI.Z); +// Inertia tensor in world space +Matrix3d ULink::GetMomentOfInertia() const { + return GetRotationMatrix() * Inertia_Tensor_Local; +} - // Moment of inertia - const Eigen::Matrix3d I0;// = R.transpose() * I * R; +Matrix3d ULink::GetRotationMatrix() const{ + const FQuat Q = this->GetRelativeRotation().Quaternion(); + return Quaterniond(Q.W, Q.X, Q.Y, Q.Z).toRotationMatrix(); +} - return I0; +void ULink::Setup(){ + SetupJoints(); + SetupProperties(); +} + +void ULink::SetupProperties(){ + Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal(); + + CenterOfMass = ToEigen(GetCenterOfMass()); + Position = ToEigen(GetComponentLocation()); + Velocity = Vector3d::Zero(); + Mass = IsBase ? INFINITY : CalculateMass(); + Orientation = Quaterniond::Identity(); + AngularVelocity = Vector3d::Zero(); + Inertia = GetMomentOfInertia(); } -Eigen::Matrix3d ULink::GetRotationMatrix() const{ - const FQuat fq = this->GetRelativeRotation().Quaternion(); - const Eigen::Vector3d q(fq.X, fq.Y, fq.Z); - const double w = fq.W; - Eigen::Matrix3d q_hat; - q_hat << 0, -q.z(), q.y(), - q.z(), 0, -q.x(), - -q.y(), q.x(), 0; - const Eigen::Matrix3d R = (w * w - q.dot(q)) * Eigen::Matrix3d::Identity() + 2 * q * q.transpose() + 2 * w * q_hat; - return R; +void ULink::SetupJoints() const { + if (const UStaticMeshSocket* Socket = GetSocketByName("JointPrev")){ + if (PrevJoint){ + PrevJoint->SecondLocalPosition = ToEigen(Socket->RelativeLocation); + PrevJoint->SecondRotateAxis = ToEigen(Socket->RelativeRotation.Vector()); + } + } + if (const UStaticMeshSocket* Socket = GetSocketByName("JointNext")){ + if (NextJoint){ + NextJoint->FirstLocalPosition = ToEigen(Socket->RelativeLocation); + NextJoint->FirstRotateAxis = ToEigen(Socket->RelativeRotation.Vector()); + } + } } -void ULink::BeginPlay(){ - const FVector t = this->GetInertiaTensor(); - inertia_tensor = Eigen::Vector3d(t.X, t.Y, t.Z); + +void ULink::Update(const double H){ + const Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass; - const double M = this->CalculateMass(); - Eigen::Vector3d v; - v << 3, 5, 7; + Last_Position = Position; + Velocity += H * ExtTransForce / Mass; + Position += H * Velocity; - UE_LOG(LogTemp, Log, TEXT("%f"), M); + 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.normalize(); +} + +void ULink::Integrate(const double H){ + Velocity = (Position - Last_Position) / H; + Quaterniond DeltaOrientation = Orientation * Last_Orientation.inverse(); + AngularVelocity = 2 * DeltaOrientation.vec() / H; + AngularVelocity = DeltaOrientation.w() >= 0 ? AngularVelocity : -AngularVelocity; +} + +void ULink::UpdateVisualTransform(){ + SetWorldLocation(FromEigen(Position)); + SetWorldRotation(FromEigen(Orientation)); } diff --git a/Source/PBDRobotics/Link.h b/Source/PBDRobotics/Link.h index 3284333..879ab91 100644 --- a/Source/PBDRobotics/Link.h +++ b/Source/PBDRobotics/Link.h @@ -3,10 +3,13 @@ #pragma once #include "CoreMinimal.h" +#include "Joint.h" #include "Components/StaticMeshComponent.h" #include "Eigen/Dense" #include "Link.generated.h" +using namespace Eigen; + /** * */ @@ -14,21 +17,41 @@ UCLASS(ClassGroup=(Custom), meta=(BlueprintSpawnableComponent)) class PBDROBOTICS_API ULink : public UStaticMeshComponent { GENERATED_BODY() - public: - Eigen::Vector3d CenterOfMass; - Eigen::Vector3d Position; - Eigen::Vector3d Velocity; + + // Properties in World space + + Vector3d CenterOfMass; + Vector3d Position; + Vector3d Velocity; double Mass; - Eigen::Quaterniond Orientation; - Eigen::Vector3d AngleVelocity; - Eigen::Matrix3d Inertia; + Quaterniond Orientation; + Vector3d AngularVelocity; + Matrix3d Inertia; + + Vector3d Last_Position; + Quaterniond Last_Orientation; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UJoint* PrevJoint; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UJoint* NextJoint; + + void Update(const double H); + void Integrate(const double H); + void UpdateVisualTransform(); + Matrix3d GetRotationMatrix() const; + + void Setup(); private: - Eigen::Vector3d inertia_tensor; - Eigen::Matrix3d GetMomentOfInertia() const; - Eigen::Matrix3d GetRotationMatrix() const; + UPROPERTY(EditAnywhere) + bool IsBase = false; + + Matrix3d Inertia_Tensor_Local; + Matrix3d GetMomentOfInertia() const; -protected: - virtual void BeginPlay() override; + void SetupJoints() const; + void SetupProperties(); }; diff --git a/Source/PBDRobotics/Robot.cpp b/Source/PBDRobotics/Robot.cpp index 55ff5b0..a0eed51 100644 --- a/Source/PBDRobotics/Robot.cpp +++ b/Source/PBDRobotics/Robot.cpp @@ -3,25 +3,38 @@ #include "Robot.h" -// Sets default values -ARobot::ARobot() -{ - // Set this actor to call Tick() every frame. You can turn this off to improve performance if you don't need it. +ARobot::ARobot(){ PrimaryActorTick.bCanEverTick = true; - } -// Called when the game starts or when spawned -void ARobot::BeginPlay() -{ +void ARobot::BeginPlay(){ Super::BeginPlay(); + GetComponents(Parts); + for (auto* Link : Parts) Link->Setup(); } -// Called every frame -void ARobot::Tick(float DeltaTime) -{ +void ARobot::Tick(const float DeltaTime){ Super::Tick(DeltaTime); + + constexpr int SubSteps = 10; + const double h = DeltaTime / SubSteps; + for (int i = 0; i < SubSteps; i++){ + for (auto* Link : Parts) Link->Update(h); + SolvePositions(); + for (auto* Link : Parts) Link->Integrate(h); + SolveVelocities(); + } + + for (auto* Link : Parts) Link->UpdateVisualTransform(); +} + +void ARobot::SolvePositions(){ + + } +void ARobot::SolveVelocities(){ + +} diff --git a/Source/PBDRobotics/Robot.h b/Source/PBDRobotics/Robot.h index 758d741..a48ba49 100644 --- a/Source/PBDRobotics/Robot.h +++ b/Source/PBDRobotics/Robot.h @@ -3,6 +3,7 @@ #pragma once #include "CoreMinimal.h" +#include "Link.h" #include "GameFramework/Actor.h" #include "Robot.generated.h" @@ -10,7 +11,6 @@ UCLASS() class PBDROBOTICS_API ARobot : public AActor { GENERATED_BODY() - public: // Sets default values for this actor's properties ARobot(); @@ -23,4 +23,10 @@ public: // Called every frame virtual void Tick(float DeltaTime) override; +private: + UPROPERTY() + TArray Parts; + + void SolvePositions(); + void SolveVelocities(); }; diff --git a/Source/PBDRobotics/util.h b/Source/PBDRobotics/util.h new file mode 100644 index 0000000..84b4fc2 --- /dev/null +++ b/Source/PBDRobotics/util.h @@ -0,0 +1,27 @@ +#pragma once + +#include "Eigen/Dense" + +inline Vector3d ToEigen(const FVector V){ + return Vector3d(V.X, V.Y, V.Z); +} + +inline FVector FromEigen(const Vector3d V){ + return FVector(V.x(), V.y(), V.z()); +} + +inline FQuat FromEigen(const Quaterniond Q){ + return FQuat(Q.x(), Q.y(), Q.z(), Q.w()); +} + +inline Quaterniond operator* (const Quaterniond Q, const double D){ + return Quaterniond(Q.w() * D, Q.x() * D, Q.y() * D, Q.z() * D); +} + +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; +}