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.
 
 

99 lines
2.8 KiB

// Fill out your copyright notice in the Description page of Project Settings.
#include "Link.h"
#include "util.h"
#include "Eigen/Dense"
#include "Engine/StaticMeshSocket.h"
using namespace Eigen;
Vector3d ULink::GetGravityForce() const{
if (IsBase)
return Vector3d::Zero();
return Vector3d(0, 0, -9.81 * 100) * Mass;
}
Matrix3d ULink::GetInertia() const {
const Matrix3d R = Orientation.toRotationMatrix();
return R * Inertia_Tensor_Local * R.transpose();
}
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;
}
void ULink::Setup(){
SetupJoints();
SetupProperties();
}
void ULink::SetupProperties(){
Mass = CalculateMass();
Position = ToEigen(GetComponentLocation());
Velocity = Vector3d::Zero();
Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal();
Orientation = ToEigen(GetComponentQuat());
AngularVelocity = Vector3d::Zero();
if (IsBase){
Mass = 1e50;
Inertia_Tensor_Local = Matrix3d::Identity() * 1e50;
}
}
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());
}
}
}
FVector ULink::GetPosition() const{
return FromEigen(Position);
}
FQuat ULink::GetOrientation() const{
return FromEigen(Orientation);
}
void ULink::Update(const double H, const Vector3d& ExtTransForce, const Vector3d& ExtRotForce){
// Translation
Last_Position = Position;
Velocity += H * ExtTransForce / Mass;
Position += H * Velocity;
Last_Orientation = Orientation;
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.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));
}