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.

97 lines
2.7 KiB

2 years ago
// Fill out your copyright notice in the Description page of Project Settings.
#include "Link.h"
#include <iostream>
#include "util.h"
2 years ago
#include "Eigen/Dense"
#include "Engine/StaticMeshSocket.h"
2 years ago
using namespace Eigen;
2 years ago
Matrix3d ULink::GetInertia() const {
return Orientation.toRotationMatrix() * Inertia_Tensor_Local;
}
2 years ago
double ULink::GetPositionalInverseMass(const Vector3d R, const Vector3d N) const{
const double M = Mass;
const Matrix3d I = GetInertia();
return 1 / M + R.cross(N).transpose() * I.inverse() * R.cross(N);
}
double ULink::GetRotationalInverseMass(const Vector3d N) const{
const Matrix3d I = GetInertia();
return N.transpose() * I.inverse() * N;
}
2 years ago
void ULink::Setup(){
SetupJoints();
SetupProperties();
}
void ULink::SetupProperties(){
Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal();
Position = ToEigen(GetComponentLocation());
Velocity = Vector3d::Zero();
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);
2 years ago
}
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());
}
}
2 years ago
}
void ULink::Update(const double H){
Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 0;
if (IsBase)
ExtTransForce = Vector3d::Zero();
2 years ago
Last_Position = Position;
Velocity += H * ExtTransForce / Mass;
Position += H * Velocity;
2 years ago
Last_Orientation = Orientation;
// Angle Velocity has no external contributor
Vector3d w = AngularVelocity;
Orientation = 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::UpdateInternalTransform(){
SetWorldLocation(FromEigen(Position));
SetWorldRotation(FromEigen(Orientation));
2 years ago
}