Solving Inverse-Kinematic Problem for PUMA via Position Based Dynamics
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 

142 lines
4.1 KiB

// Fill out your copyright notice in the Description page of Project Settings.
#include "Joint.h"
#include "Link.h"
#include "util.h"
#include "Kismet/KismetMathLibrary.h"
Vector3d UJoint::GetFirstWorldDirection() const{
return FirstLink->Orientation * FirstLocalPosition;
}
Vector3d UJoint::GetSecondWorldDirection() const{
return SecondLink->Orientation * SecondLocalPosition;
}
Vector3d UJoint::GetConnection() const{
return FirstLink->Position + GetFirstWorldDirection() - (SecondLink->Position + GetSecondWorldDirection());
}
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 = GetFirstWorldDirection();
const Vector3d R2 = GetSecondWorldDirection();
const Vector3d D = GetConnection();
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);
constexpr double A = 0; //1. / 100000000; // Compliance (inverse of stiffness)
const double Denominator = W1 + W2 + A / (H * H);
Vector3d P;
if (Denominator == 0)
P = Vector3d::Zero();
else
P = N * -C / Denominator;
L1->Position += P / M1;
L2->Position -= P / M2;
Vector3d T1 = I1.inverse() * R1.cross(P);
Vector3d T2 = I2.inverse() * R2.cross(P);
const 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;
L1->Orientation = (L1->Orientation + Add1).normalized();
L2->Orientation = (L2->Orientation - Add2).normalized();
}
// Rotational Constraints
{
Vector3d Q;
if (L2->IsEffector){
const Quaterniond Tq = L1->Orientation * L2->Orientation.inverse();
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;
const Matrix3d I1 = L1->GetInertia();
const Matrix3d I2 = L2->GetInertia();
const double W1 = L1->GetRotationalInverseMass(N);
const double W2 = L2->GetRotationalInverseMass(N);
constexpr double A = 0; //1. / 100000000;
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;
const 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;
L1->Orientation = (L1->Orientation + Add1).normalized();
L2->Orientation = (L2->Orientation - Add2).normalized();
}
}
void UJoint::SolveVelocity(const double H) const{
ULink* L1 = FirstLink;
ULink* L2 = SecondLink;
// Damping
const Vector3d V1 = L1->Velocity;
const Vector3d V2 = L2->Velocity;
constexpr double MuLin = 10000; // Damping strength
const Vector3d DeltaV = (V2 - V1) * (MuLin * H < 1 ? MuLin * H : 1);
const Vector3d R1 = GetFirstWorldDirection();
const Vector3d R2 = GetSecondWorldDirection();
const Vector3d N = (R2 - R1).normalized();
const Matrix3d I1 = L1->GetInertia();
const Matrix3d I2 = L2->GetInertia();
const double W1 = L1->GetPositionalInverseMass(R1, N);
const double W2 = L2->GetPositionalInverseMass(R2, N);
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());
//L1->Velocity += P / L1->Mass;
//L2->Velocity -= P / L2->Mass;
//L1->AngularVelocity += I1.inverse() * R1.cross(P);
//L2->AngularVelocity -= I2.inverse() * R2.cross(P);
}
UJoint::UJoint()
{
}