Robotics
/
PBD-Robotics
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: Robotics:e91dc13878f83b0077770def33fd90d28be163a0
Branches Tags
Robotics:main
..
compare: Robotics:ac746fcbaf650be84441dc6d95d9cec0bd0dd2c3
Branches Tags
Robotics:main

No commits in common. 'e91dc13878f83b0077770def33fd90d28be163a0' and 'ac746fcbaf650be84441dc6d95d9cec0bd0dd2c3' have entirely different histories.
e91dc13878 ... ac746fcbaf

12 changed files with 46 additions and 141 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats
  1. BIN
      Content/Blueprints/BP_TestBot.uasset
  2. BIN
      Content/Blueprints/BP_TestCube.uasset
  3. BIN
      Content/Levels/Main.umap
  4. 26
      Content/Models/arm.obj
  5. 26
      Content/Models/cube.obj
  6. BIN
      Content/Models/test_arm.uasset
  7. BIN
      Content/Models/test_base.uasset
  8. 63
      Source/PBDRobotics/Joint.cpp
  9. 14
      Source/PBDRobotics/Joint.h
  10. 31
      Source/PBDRobotics/Link.cpp
  11. 4
      Source/PBDRobotics/Robot.cpp
  12. 23
      Source/PBDRobotics/util.h

BIN
Content/Blueprints/BP_TestBot.uasset
View File

Binary file not shown.

BIN
Content/Blueprints/BP_TestCube.uasset
View File

Binary file not shown.

BIN
Content/Levels/Main.umap
View File

Binary file not shown.

26
Content/Models/arm.obj
Unescape View File

@ -1,26 +0,0 @@
# Blender v2.76 (sub 0) OBJ File: ''
# www.blender.org
mtllib cube.mtl
o Cube
v 1.000000 -1.000000 -4.000000
v 1.000000 -1.000000 4.000000
v -1.000000 -1.000000 4.000000
v -1.000000 -1.000000 -4.000000
v 1.000000 1.000000 -4.000000
v 1.000000 1.000000 4.00000
v -1.000000 1.000000 4.000000
v -1.000000 1.000000 -4.000000
usemtl Material
s off
f 2/1/1 3/2/1 4/3/1
f 8/1/2 7/4/2 6/5/2
f 5/6/3 6/7/3 2/8/3
f 6/8/4 7/5/4 3/4/4
f 3/9/5 7/10/5 8/11/5
f 1/12/6 4/13/6 8/11/6
f 1/4/1 2/1/1 4/3/1
f 5/14/2 8/1/2 6/5/2
f 1/12/3 5/6/3 2/8/3
f 2/12/4 6/8/4 3/4/4
f 4/13/5 3/9/5 8/11/5
f 5/6/6 1/12/6 8/11/6

26
Content/Models/cube.obj
Unescape View File

@ -1,26 +0,0 @@
# Blender v2.76 (sub 0) OBJ File: ''
# www.blender.org
mtllib cube.mtl
o Cube
v 1.000000 -1.000000 -1.000000
v 1.000000 -1.000000 1.000000
v -1.000000 -1.000000 1.000000
v -1.000000 -1.000000 -1.000000
v 1.000000 1.000000 -1.000000
v 1.000000 1.000000 1.000000
v -1.000000 1.000000 1.000000
v -1.000000 1.000000 -1.000000
usemtl Material
s off
f 2/1/1 3/2/1 4/3/1
f 8/1/2 7/4/2 6/5/2
f 5/6/3 6/7/3 2/8/3
f 6/8/4 7/5/4 3/4/4
f 3/9/5 7/10/5 8/11/5
f 1/12/6 4/13/6 8/11/6
f 1/4/1 2/1/1 4/3/1
f 5/14/2 8/1/2 6/5/2
f 1/12/3 5/6/3 2/8/3
f 2/12/4 6/8/4 3/4/4
f 4/13/5 3/9/5 8/11/5
f 5/6/6 1/12/6 8/11/6

BIN
Content/Models/test_arm.uasset
View File

Binary file not shown.

BIN
Content/Models/test_base.uasset
View File

Binary file not shown.

63
Source/PBDRobotics/Joint.cpp
Unescape View File

@ -7,37 +7,30 @@
#include "util.h" #include "util.h"
#include "Kismet/KismetMathLibrary.h" #include "Kismet/KismetMathLibrary.h"
Vector3d UJoint::GetFirstWorldDirection() const{ Vector3d UJoint::GetFirstWorldPosition() const{
return FirstLink->Orientation * FirstLocalPosition; return FirstLink->Position + FirstLink->Orientation * FirstLocalPosition;
} }
Vector3d UJoint::GetSecondWorldDirection() const{ Vector3d UJoint::GetSecondWorldPosition() const{
return SecondLink->Orientation * SecondLocalPosition; return SecondLink->Position + SecondLink->Orientation * SecondLocalPosition;
}
Vector3d UJoint::GetConnection() const{
return FirstLink->Position + GetFirstWorldDirection() - (SecondLink->Position + GetSecondWorldDirection());
} }
Vector3d UJoint::GetFirstWorldAxis() const{ Vector3d UJoint::GetFirstWorldAxis() const{
return FirstLink->Orientation * FirstRotateAxis; return FirstLink->Orientation * FirstRotateAxis;
} }
Vector3d UJoint::GetSecondWorldAxis() const{ Vector3d UJoint::GetSecondWorldAxis() const{
return SecondLink->Orientation * SecondRotateAxis; return SecondLink->Orientation * SecondRotateAxis;
} }
void UJoint::SolvePosition(const double H) const{ void UJoint::SolvePosition(const double H) const{
ULink* L1 = FirstLink; ULink* L1 = FirstLink;
ULink* L2 = SecondLink; ULink* L2 = SecondLink;
// Positional Constraints // Positional Constraints
{ {
const Vector3d R1 = GetFirstWorldDirection(); const Vector3d R1 = GetFirstWorldPosition();
const Vector3d R2 = GetSecondWorldDirection(); const Vector3d R2 = GetSecondWorldPosition();
const Vector3d D = GetConnection(); const Vector3d D = R1 - R2; // Original PBD (not R2 - R1), because Impulse P is negated
const Vector3d N = D.normalized(); const Vector3d N = D.normalized();
const double C = D.norm(); const double C = D.norm();
const Matrix3d I1 = L1->GetInertia(); const Matrix3d I1 = L1->GetInertia();
@ -49,7 +42,6 @@ void UJoint::SolvePosition(const double H) const{
const double W2 = L2->GetPositionalInverseMass(R2, N); const double W2 = L2->GetPositionalInverseMass(R2, N);
constexpr double A = 1. / 10000000; // Compliance (inverse of stiffness) constexpr double A = 1. / 10000000; // Compliance (inverse of stiffness)
const Vector3d P = -C / (W1 + W2 + A / (H * H)) * N; const Vector3d P = -C / (W1 + W2 + A / (H * H)) * N;
L1->Position += P / M1; L1->Position += P / M1;
@ -57,21 +49,20 @@ void UJoint::SolvePosition(const double H) const{
Vector3d T1 = I1.inverse() * R1.cross(P); Vector3d T1 = I1.inverse() * R1.cross(P);
Vector3d T2 = I2.inverse() * R2.cross(P); Vector3d T2 = I2.inverse() * R2.cross(P);
const Quaterniond Add1 = Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5; Quaterniond Add1 = Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5;
const Quaterniond Add2 = Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5; Quaterniond Add2 = Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;
L1->Orientation = (L1->Orientation + Add1).normalized(); //L1->Orientation = L1->Orientation + Add1;
L2->Orientation = (L2->Orientation - Add2).normalized(); //L2->Orientation = L2->Orientation- Add2;
UE_LOG(LogTemp, Log, TEXT("%f | %f | %f"), C, P.norm(), R2.cross(P).norm()); //UE_LOG(LogTemp, Log, TEXT("%f %f"), (T1).norm(), T2.norm());
} }
// Rotational Constraints // Rotational Constraints
{ {
const Vector3d A1 = GetFirstWorldAxis(); const Vector3d A1 = GetFirstWorldAxis();
const Vector3d A2 = GetSecondWorldAxis(); const Vector3d A2 = GetSecondWorldAxis();
assert(A1.norm() == A2.norm() == 1);
const Vector3d Q = A1.cross(A2); const Vector3d Q = A1.cross(A2);
const Vector3d N = Q.normalized(); const Vector3d N = Q.normalized();
const double Theta = UKismetMathLibrary::Asin(Q.norm());// * 180. / PI; const double Theta = Q.norm();
const Matrix3d I1 = L1->GetInertia(); const Matrix3d I1 = L1->GetInertia();
const Matrix3d I2 = L2->GetInertia(); const Matrix3d I2 = L2->GetInertia();
@ -79,16 +70,16 @@ void UJoint::SolvePosition(const double H) const{
const double W1 = L1->GetRotationalInverseMass(N); const double W1 = L1->GetRotationalInverseMass(N);
const double W2 = L2->GetRotationalInverseMass(N); const double W2 = L2->GetRotationalInverseMass(N);
constexpr double A = 0; //1. / 10000000; const double A = 0.000001 / (H * H);
const Vector3d P = Theta / (W1 + W2 + A / (H * H)) * N; const Vector3d P = -Theta / (W1 + W2 + A) * N;
Vector3d T1 = I1.inverse() * P; Vector3d T1 = I1.inverse() * P;
Vector3d T2 = I2.inverse() * P; Vector3d T2 = I2.inverse() * P;
const Quaterniond Add1 = Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5; T1 = T2 = P;
const Quaterniond Add2 = Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5; //L1->Orientation += Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5;
//L1->Orientation = (L1->Orientation + Add1).normalized(); //L2->Orientation -= Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;
//L2->Orientation = (L2->Orientation - Add2).normalized();
//UE_LOG(LogTemp, Log, TEXT("%f | %f | %f | %f"), N.norm(), W1, W2, Q.norm()); // UE_LOG(LogTemp, Log, TEXT("%f | %f"), W1, W2);
} }
} }
@ -101,11 +92,11 @@ void UJoint::SolveVelocity(const double H) const{
const Vector3d V1 = L1->Velocity; const Vector3d V1 = L1->Velocity;
const Vector3d V2 = L2->Velocity; const Vector3d V2 = L2->Velocity;
constexpr double MuLin = 10000; // Damping strength constexpr double MuLin = 100000; // Damping strength
const Vector3d DeltaV = (V2 - V1) * (MuLin * H < 1 ? MuLin * H : 1); const Vector3d DeltaV = (V2 - V1) * UKismetMathLibrary::Min(MuLin * H, 1);
const Vector3d R1 = GetFirstWorldDirection(); const Vector3d R1 = GetFirstWorldPosition();
const Vector3d R2 = GetSecondWorldDirection(); const Vector3d R2 = GetSecondWorldPosition();
const Vector3d N = (R2 - R1).normalized(); const Vector3d N = (R2 - R1).normalized();
const Matrix3d I1 = L1->GetInertia(); const Matrix3d I1 = L1->GetInertia();
@ -116,10 +107,10 @@ void UJoint::SolveVelocity(const double H) const{
const Vector3d P = DeltaV / (W1 + W2); const Vector3d P = DeltaV / (W1 + W2);
//UE_LOG(LogTemp, Log, TEXT("%f | %f | %f | %f | %f"), P.norm(), MuLin * H, DeltaV.norm(), (V2 - V1).norm()); // UE_LOG(LogTemp, Log, TEXT("%f | %f"), (V2 - V1).norm(), MuLin * H);
//L1->Velocity += P / L1->Mass; L1->Velocity += P / L1->Mass;
//L2->Velocity -= P / L2->Mass; L2->Velocity -= P / L2->Mass;
//L1->AngularVelocity += I1.inverse() * R1.cross(P); //L1->AngularVelocity += I1.inverse() * R1.cross(P);
//L2->AngularVelocity -= I2.inverse() * R2.cross(P); //L2->AngularVelocity -= I2.inverse() * R2.cross(P);
} }

14
Source/PBDRobotics/Joint.h
Unescape View File

@ -24,22 +24,12 @@ public:
UPROPERTY(BlueprintReadWrite) UPROPERTY(BlueprintReadWrite)
ULink* SecondLink; ULink* SecondLink;
// First Connection point in local space
Vector3d FirstLocalPosition; Vector3d FirstLocalPosition;
// Second Connection point in local space
Vector3d SecondLocalPosition; Vector3d SecondLocalPosition;
// Direction from CenterOfMass to First Connection point Vector3d GetFirstWorldPosition() const;
Vector3d GetFirstWorldDirection() const; Vector3d GetSecondWorldPosition() const;
// Direction from CenterOfMass to Second Connection point
Vector3d GetSecondWorldDirection() const;
// First - Second in World-Space
Vector3d GetConnection() const;
//
Vector3d FirstRotateAxis; Vector3d FirstRotateAxis;
Vector3d SecondRotateAxis; Vector3d SecondRotateAxis;

31
Source/PBDRobotics/Link.cpp
Unescape View File

@ -12,8 +12,7 @@
using namespace Eigen; using namespace Eigen;
Matrix3d ULink::GetInertia() const { Matrix3d ULink::GetInertia() const {
const Matrix3d R = Orientation.toRotationMatrix(); return Orientation.toRotationMatrix() * Inertia_Tensor_Local;
return R * Inertia_Tensor_Local * R.transpose();
} }
double ULink::GetPositionalInverseMass(const Vector3d R, const Vector3d N) const{ double ULink::GetPositionalInverseMass(const Vector3d R, const Vector3d N) const{
@ -32,21 +31,25 @@ void ULink::Setup(){
} }
void ULink::SetupProperties(){ void ULink::SetupProperties(){
Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal();
Mass = CalculateMass();
Position = ToEigen(GetComponentLocation()); Position = ToEigen(GetComponentLocation());
Velocity = Vector3d::Zero(); Velocity = Vector3d::Zero();
Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal(); Mass = CalculateMass();
//Inertia_Tensor_Local = Matrix3d::Identity() * Mass;
Orientation = ToEigen(GetComponentQuat()); Orientation = ToEigen(GetComponentQuat());
AngularVelocity = Vector3d::Zero(); AngularVelocity = Vector3d::Zero();
if (IsBase){ if (IsBase){
Mass = 1e50; Mass = 1e30;
Inertia_Tensor_Local = Vector3d::Ones().asDiagonal() * 1e50; Inertia_Tensor_Local = Vector3d::Ones().asDiagonal() * 1e30;
} }
Log(GetInertia()); 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 { void ULink::SetupJoints() const {
@ -65,9 +68,7 @@ void ULink::SetupJoints() const {
} }
void ULink::Update(const double H){ void ULink::Update(const double H){
Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 0;
// Translation
Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 1;
if (IsBase) if (IsBase)
ExtTransForce = Vector3d::Zero(); ExtTransForce = Vector3d::Zero();
@ -75,13 +76,9 @@ void ULink::Update(const double H){
Velocity += H * ExtTransForce / Mass; Velocity += H * ExtTransForce / Mass;
Position += H * Velocity; Position += H * Velocity;
// Rotation
const Vector3d ExtRotForce = Vector3d::Zero();
Last_Orientation = Orientation; Last_Orientation = Orientation;
// Angle Velocity has no external contributor
Vector3d w = AngularVelocity; Vector3d w = AngularVelocity;
AngularVelocity += H * GetInertia().inverse() * (ExtRotForce - w.cross(GetInertia() * w));
w = AngularVelocity;
Orientation = 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(); Orientation.normalize();
} }

4
Source/PBDRobotics/Robot.cpp
Unescape View File

@ -3,8 +3,6 @@
#include "Robot.h" #include "Robot.h"
#include "util.h"
ARobot::ARobot(){ ARobot::ARobot(){
PrimaryActorTick.bCanEverTick = true; PrimaryActorTick.bCanEverTick = true;
@ -27,7 +25,7 @@ void ARobot::BeginPlay(){
void ARobot::Tick(const float DeltaTime){ void ARobot::Tick(const float DeltaTime){
Super::Tick(DeltaTime); Super::Tick(DeltaTime);
constexpr int SubSteps = 10; constexpr int SubSteps = 30;
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 : Parts)

23
Source/PBDRobotics/util.h
Unescape View File

@ -1,7 +1,6 @@
#pragma once #pragma once
#include "Eigen/Dense" #include "Eigen/Dense"
#include <string>
inline Vector3d ToEigen(const FVector V){ inline Vector3d ToEigen(const FVector V){
return Vector3d(V.X, V.Y, V.Z); return Vector3d(V.X, V.Y, V.Z);
@ -31,24 +30,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()); return Quaterniond(Q1.w() - Q2.w(), Q1.x() - Q2.x(), Q1.y() - Q2.y(), Q1.z() - Q2.z());
} }
inline void Log(MatrixXd M){ inline Quaterniond operator-= (const Quaterniond Q1, const Quaterniond Q2){
assert(M.rows() == 3); return Q1 - Q2;
if (M.cols() == 3){
UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(0, 0), M(0, 1), M(0, 2));
UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(1, 0), M(1, 1), M(1, 2));
UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(2, 0), M(2, 1), M(2, 2));
} }
if (M.cols() == 1){
UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(0), M(1), M(2));
}
}
inline void Log(const double V){
UE_LOG(LogTemp, Log, TEXT("%f"), V);
}
inline void Log(const char* S){
UE_LOG(LogTemp, Log, TEXT("%s"), *FString(S));
}

Write Preview
Loading…
Cancel
Save