Added reset of position and orientation at the start

main
Benjamin Kraft 2 years ago
parent 0280aad4db
commit d0bee4a37a
  1. 1
      Config/DefaultEngine.ini
  2. BIN
      Content/Blueprints/BP_Puma.uasset
  3. BIN
      Content/Blueprints/BP_Robot.uasset
  4. BIN
      Content/Blueprints/BP_TestBot.uasset
  5. BIN
      Content/Blueprints/BP_TestCube.uasset
  6. BIN
      Content/Levels/Main.umap
  7. 2
      Source/PBDRobotics/Joint.cpp
  8. 2
      Source/PBDRobotics/Link.cpp
  9. 41
      Source/PBDRobotics/Robot.cpp
  10. 21
      Source/PBDRobotics/Robot.h

@ -24,3 +24,4 @@ r.DefaultFeature.AutoExposure=False
+ClassRedirects=(OldName="/Script/PBDRobotics.Joint",NewName="/Script/PBDRobotics.Joint") +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.PrevLink",NewName="/Script/PBDRobotics.Joint.FirstLink")
+PropertyRedirects=(OldName="/Script/PBDRobotics.Joint.NextLink",NewName="/Script/PBDRobotics.Joint.SecondLink") +PropertyRedirects=(OldName="/Script/PBDRobotics.Joint.NextLink",NewName="/Script/PBDRobotics.Joint.SecondLink")
+PropertyRedirects=(OldName="/Script/PBDRobotics.Robot.Parts",NewName="/Script/PBDRobotics.Robot.Links")

Binary file not shown.

Binary file not shown.

@ -72,7 +72,7 @@ void UJoint::SolvePosition(const double H) const{
Vector3d Q; Vector3d Q;
if (L2->IsEffector){ if (L2->IsEffector){
const Quaterniond Tq = L1->Orientation * L2->Orientation.inverse(); const Quaterniond Tq = L1->Orientation * L2->Orientation.inverse();
Q = 2 * Tq.vec(); Q = -2 * Tq.vec();
} else{ } else{
const Vector3d A1 = GetFirstWorldAxis(); const Vector3d A1 = GetFirstWorldAxis();
const Vector3d A2 = GetSecondWorldAxis(); const Vector3d A2 = GetSecondWorldAxis();

@ -45,8 +45,6 @@ void ULink::SetupProperties(){
Mass = 1e50; Mass = 1e50;
Inertia_Tensor_Local = Vector3d::Ones().asDiagonal() * 1e50; Inertia_Tensor_Local = Vector3d::Ones().asDiagonal() * 1e50;
} }
Log(GetInertia());
} }
void ULink::SetupJoints() const { void ULink::SetupJoints() const {

@ -3,9 +3,6 @@
#include "Robot.h" #include "Robot.h"
#include "util.h"
ARobot::ARobot(){ ARobot::ARobot(){
PrimaryActorTick.bCanEverTick = true; PrimaryActorTick.bCanEverTick = true;
} }
@ -13,32 +10,50 @@ ARobot::ARobot(){
void ARobot::BeginPlay(){ void ARobot::BeginPlay(){
Super::BeginPlay(); Super::BeginPlay();
GetComponents<ULink>(Parts); for (auto* Link : Links)
for (auto* Link : Parts){
Link->Setup(); Link->Setup();
if (Link->PrevJoint) ResetPositionAndOrientation();
Joints.Add(Link->PrevJoint); }
void ARobot::ResetPositionAndOrientation(){
for (const auto* Joint : Joints){
const ULink* L1 = Joint->FirstLink;
ULink* L2 = Joint->SecondLink;
Vector3d ConnectPosL1 = L1->Position + Joint->GetFirstWorldDirection();
const Vector3d Correction = ConnectPosL1 - Joint->GetSecondWorldDirection();
L2->Position = Correction;
}
} }
// Example: 3 Parts are connected with 2 Joints
assert(Parts.Num() == Joints.Num() + 1); void ARobot::Connect(ULink* L1, ULink* L2){
Links.AddUnique(L1);
Links.Add(L2);
UJoint* Joint = NewObject<UJoint>(this);
Joint->FirstLink = L1;
Joint->SecondLink = L2;
L1->NextJoint = Joint;
L2->PrevJoint = Joint;
Joints.Add(Joint);
} }
void ARobot::Tick(const float DeltaTime){ void ARobot::Tick(const float DeltaTime){
Super::Tick(DeltaTime); Super::Tick(DeltaTime);
constexpr int SubSteps = 30; constexpr int SubSteps = 50;
const double h = DeltaTime / SubSteps; const double h = DeltaTime / SubSteps;
for (int i = 0; i < SubSteps; i++){ for (int i = 0; i < SubSteps; i++){
for (auto* Link : Parts) for (auto* Link : Links)
Link->Update(h); Link->Update(h);
for (const auto* Joint : Joints) for (const auto* Joint : Joints)
Joint->SolvePosition(h); Joint->SolvePosition(h);
for (auto* Link : Parts) for (auto* Link : Links)
Link->Integrate(h); Link->Integrate(h);
for (const auto* Joint : Joints) for (const auto* Joint : Joints)
Joint->SolveVelocity(h); Joint->SolveVelocity(h);
} }
for (auto* Link : Parts) for (auto* Link : Links)
Link->UpdateInternalTransform(); Link->UpdateInternalTransform();
} }

@ -12,21 +12,22 @@ class PBDROBOTICS_API ARobot : public AActor
{ {
GENERATED_BODY() GENERATED_BODY()
public: public:
// Sets default values for this actor's properties
ARobot(); ARobot();
protected: protected:
// Called when the game starts or when spawned
virtual void BeginPlay() override; virtual void BeginPlay() override;
public: UPROPERTY(BlueprintReadWrite)
// Called every frame TArray<ULink*> Links = TArray<ULink*>();
virtual void Tick(float DeltaTime) override;
private: UPROPERTY(BlueprintReadWrite)
UPROPERTY()
TArray<ULink*> Parts;
UPROPERTY()
TArray<UJoint*> Joints = TArray<UJoint*>(); TArray<UJoint*> Joints = TArray<UJoint*>();
UFUNCTION(BlueprintCallable)
void Connect(ULink* L1, ULink* L2);
private:
void ResetPositionAndOrientation();
public:
virtual void Tick(float DeltaTime) override;
}; };

Loading…
Cancel
Save