From 322aa8baa21eb8e38e620aa28d71ece4603e7873 Mon Sep 17 00:00:00 2001 From: Benjamin Kraft Date: Tue, 11 Oct 2022 14:53:11 +0200 Subject: [PATCH] Before Input --- Content/Blueprints/BP_Robot.uasset | Bin 35417 -> 38150 bytes Content/Levels/Main.umap | Bin 636527 -> 636527 bytes Source/PBDRobotics/Link.cpp | 9 ++++++--- Source/PBDRobotics/Link.h | 4 +++- Source/PBDRobotics/Robot.cpp | 21 +++++++++++---------- Source/PBDRobotics/Robot.h | 4 ++++ Source/PBDRobotics/util.h | 13 ++++++------- 7 files changed, 30 insertions(+), 21 deletions(-) diff --git a/Content/Blueprints/BP_Robot.uasset b/Content/Blueprints/BP_Robot.uasset index 6ba2ab34cf27beeea5ef577cee631233349577bd..135280223908d9fa768804f5a194f11a750c50ce 100644 GIT binary patch delta 8694 zcmcIp3tUsz89z6I2vrJ%B!q_q1ZF@i34tW|gb+Xxs@5tBf&l_Xkw*v?Uxli**tPZX zwY6xwVr%VFr^TvUf19@5I=7Ck-R!n*ZnNv!xz0}7t&hEQ=Q}y)8c4$2>+rid=ls9t z{l4?Mhj$)S?08PG`%p}}gOE@{NM&2AlU5<***zys6>8f2R7J=XT8Bpy^8RE(B-;Kc znd0<`))8_k;}>buRc+-*UVUTud}M1$_kp-qZ@Ms0_QXp&UXMxlXpkTEJPT&&_M`?X zp?HXPeabaj(0)9;AtereTWStT(Nc1;v&qrb;;E}Eb*yu?w>UQ3=5csj4V~o42QptB zoHl;exbi5{&>4Io-ClR*$=_rN!wc zYxW*}8+yY(nHWMDT&*5*KIVjK&>c~zh;OT3<81H{XZgNsVU&2Q=zb)@(8QOrX-tTp zI@9@N?L?U{ueH(HO?DpWdtz|wq+cm4N=jMQ=5A?32b|p=uJXjxXRjL-zO~Vgz2dzR#Cu(!2EC@ApOY7b&t=AiNg& z5>dKBc{ac*hh~!S{5^`?f=G|>6Oo)CdU`pIm4dgsc`a=9@;+7q8;b{rG%Yd2j|rk< zl|6o_m!18N=k2k0a8h6qJVHhU4+ClNvdBEb?FR|TXC#ctj^Ip;@*B$I5(34F=*TMoi;T`=HYY4uKGDlp^L1Unm9{QBew4 zRIrJWU(lvWjqsLMsi^K@K#MvSrp8C9ab-XsQAWY7yw}TEHpiz)&2TVYsd#^zEb|4G zS?wcp4)+aOe!PRREJ#R`wn9gOQgPrumY`z^aeI@Z)Fv+%Tih~uyA5QmmzyTAWdl9g z3x$bF#X~YNpJLVPfXN>Vkq}PbdpkW&x6813VMmM7!_>eW`K-1 zV)510urnzk``6SG_HLpNuHlpDgS!bX^tjugj}i1i>ya>&G-c%nR01i;f%=g8JBlF( zYQY*cVU60DB(0HGXvVhCju}8*NDGG)l`OQxkyJ)!-#99Yf1eN)tDU?r!NsCm2g_5ArNZ5F3wqKy zTt@0iD`_GfWF2j7Buhvat-Ht?(nvgH1}USoF0zPIn@A@qCheq+Vx6>JNBYdY!cZJ^XD&LN^pB*iGi0CP+9(V-k& z!$KsH6{tTehNHgv1BbC1dQ(baD18FhigTnIs7lR+r;G2DR9;p&?SLlgSV7LMZuYh$Z-~u3*TEh zh1Mn*UU~-E+4m{FBD#mYP@ zAfq)4-{>HLpDi~~sk7zTgXR^vQo$5DU9SG*Ob>2cIH|B~^HyXtuld4B$cJ?Jh4sggmZsSe~Zxy4kzE9a70_IRJ$8 z&Jo^?HlpFoXyleL4aD8H{*!aB>6brhyCFkg;fh5dWp8lFULuH?4k)WljTWzj3n-Rl z7?Pyn;X6<6e5q;P_Ag#8c<4t*>cv|ihZPgjBh<4zVLZmn?Gq?Si;$9?!u`WnIgT03 z7!EL`VOzL(khZZ%L`R^cPOX*&e?_H)>RfQwSu87?!nANj41gUNEvXv{RwefopdgkR-iqyI*zD(h+>==G-E^`IvN7Of@g{PR z=qp^igA;Kx#FFN%;i;7saq7k8?sTENax`&gDI_oHrKn+D~o;C7_GZbew46M58HOyh%5F z+?cLi%msx{CA{B;^Ps1d_xuJhe%h_$L_eDwI6+wI;(Z6N0!xzpdny~Pu8PdbBaZ@(9n+)L_xkNX-lAvS>pG~!4tKND6CrMzbo?Sp=e@eoEp|3J-OknqIt#BSDaD8KmHuDVc5KE`n>IRx$o`!RNoQ^yPGS+iYdk`9`99{j$L_0%%j&^ z&GdGS)NxicD8?TBiV4eMdt(j^bmp^=M5P^V-gEPX!httlxbpdiqmQH6(b#=l7go$H zPRhkjPnWw@x75+n<&5AyT)*D)OH)LISD;hpS?$!d<85G`Th}1p2kIOiM(J#KHn^Hx z&PFefTjyd|dD8VH?@5%CY-q32&g0IB$va|gnG{zW0K4p%tnL5jNir`rV3vTaUl%KQTJbCYed;Y%v;m7X! ztR)cE108E)BP84!uwa5?L-BQX($DT1J&H`P`u4@E+qM)Pa&#>GbPsA0hk zwCZx$(V?^5Wv6qdxs0dApACcsjej4DKZ?Zv^0x1*W)0c;-WdM+ht*Aw1j2&HUytlV zwJ(Bk*B>iSC(mns_u`>hfv_#`(>6=0JTs^i790xR7XSVKCG}p}!k9@Npl5^seNv&o z9W`PiiA3UsPuSQ41@(u5=npe#hu)}CEGCf`J;zl>c7-BClD4qM4A8cEGnPtV4*WMKj^}d zLzmGpU!vmedh>T4_JkEaS3b4la9{cQKx+VgYpvJ+q5t^&yvlz4snVyvzWVM!*ge4( zPZR9wnIJ{OwmZyv6m+~jCv@Jg3rde@OZT4){i-Dp_T^y8YM6@MwbL$ZKCfS}OS+&a ztbW9dDn?^2R53-`Pf1wBtKnya7vaC&Gw7$?Htk`Kh;37BX6#5Fzm8M=PTf_|u{kAL z=)b|-&_njvxdF|_kpoQZiFgt({N9xH%e#l{r=HsP^S>^By*3b51#k7LbY@e&)nqc5 zOoe$l7E6xNT4FQX&4oo4gT+{6wUt2s#>t}&+zvge`{w+T^Zm20+6KD6wQE`AqTPWo zJeeB(7F+zrn0=DEuJ5Zl%Ve!J-O4_Xq?%4qCP97xGl zi!rCzUTig$*o_7{lI%Ifxy8BEtTit`&uW6uyVi~xM=5Z$b1Jh!V)l~z&&{9v=wr^) zZ(hm?Lo1`Pe|s#6;efjEgpP|WF2Y6#+p3>|$2-=eH5U9&i^tvNl>Y@{WUvnbHzBY5 z+O64e@t)a|1(N$zQU}y-(@O(T*Jm6&x4l=Q@Vp&K0v_D6qgn#>_8Bo2AB9U?4Qq)I zynnk59^HD7%FW)EEaaeX+nu3k?k;$^uSEVHMIF8`mc^C&-2>od+_Yds{721~IPp<1 z@66La#d~6D!`Y05-BH40JENwrVtqOdcr8x!B$oreZasjvQ<1_c7BFV2`%F!%c^I3fYSUKpwJCJYy)(DF?1K4{4BusD z{`t?DGv~~lJ9oFf=yh?o*RI2X>&gk4KnN*mYP8d8JUP@oU?>iz&C5ZAM9{iYNl0xN zAp*9;X`4PB^wACd!ZOJ?koD5utmZ-O4eyqxL*BaU(!sn#C%3*67~@bQAN3r9G@&%? z11eFvlXjiTFfBNLD7-67>wjCQ8Q)**ojy)InR0UNjrLVGhrK$d!M1ue*{Ud81n>KP z)&GZySze*@me$*=9K>GuH^GN;n|OE6FeLh&>{t5#$t!Fen@G#*Yjm=ESla=O0ct-) zD%_AIZb+pYVsk^P+>mM)Bj{xM0oU9oUQ-cpxvKYmJAuVWbJaHF4KIC`kv z6DNd%ob(wb;ep2Dfa}3=UhB$1zit-vsX}7^K?lF2k2Fk3Bx~SaTa4$HmSYvry&CEF zcquADSeF30uwY0~D^PTQL6B`;|Q9b&I12UHP;BsLzOB#|C|!f z)S2qsC^#W%naQ-+8{8-;jFd=5>0BXG(x)m_m2`+tw4IrI)FD%jP>Rio;>B!9ZI&s1 z)0C?i*63XC8ev_~m78E=rJOB$4bKydi0)`35e;Xx7iPglU#h2e0sFcy;XW1%Ed(QhtNDB)iB8J;VeLTdx;D2me>NeYP~ z@$@m$=Wel1Wr&{WWGoG4&yS%}Mi7eUKxMfC8W*bIH;WcQ=8|a?H-S(Qd>8Ujmx<`1 zZHZn;>Oa1uQV58k{u%n<#trlnmNwWBq>d(-+(IRUJj7~t3Z-~-BhOph5c}cyZgpS_ zV^OzB`)3})LJPcZn_1P!2Qg#`ti#i@mGx^`&3Abxv*}5Oi~E3+I9SF=>0ncbXy!61 zAB|@H9vp?9rO}!lBN(`=u{=i^5p((ds#pTWbHJMI8Qk8Hlc*U(n#5eC9&?hA#OAMF zJ-hIPA?I67wpa2q9>j&%gzHr^0{HT9^J7^gE^ltN(vRDSsL?$xxLT*kJ%bt(#S`r; zh{4BmhNl@%JGS|hPs7#fNR`aKLa99RNf2f?waG&x^BsR*p+^2<>>pU%@|F?!mubtuaWY?=F`*JbAo_cktiI3gQ8klOZ0E9n0UFLS8FLDSq zWO%hSF6JO?MzB6#sOT8f;xWy^f|@YRy-b9=%_UM{14jxUtf^d>d*Z~wpWL2S+{^7n zA<+(R$qo#lzdS2C#_!JB;n^D3ec%ASYKkAakDp{ST&jtbS56?kc2)!fvS3g>|F;OtdG$wV1nEvE9)=AlPlEY-fJUx zyRREO)r*4URb82kvY9=j<5^#D-s~G zK_B%96XBDir3{iPhxm%^3sb;n%`tGkp^U1)mEr&Cuz!nIm<*ejFA=uFC(E1M#*}cf zF98OF8I3eyk&BDPtmq-_C2+KHP|;rl)9kFAr^Q%|2{fxp->#ya}GLJ))+3S(D^z{K#OMzP~p&$;zoWGe+G} z7bq>0QGY;JztQ6AYD`~PFy)qg>2q}OOiSKrU3^{ADep6$Fi)=D##WRRE!?SgEYZribO(wz&L)s9<#}X7Iloz?YaPC9hCG%kyjgMExU}_dL-s4Y8@_7 zL#ea5g%`g1?Vh3z?H_MlDvgcvgxxu6D0MO39M@Cx`{EaSYa)MFu&3D*mI^=Hp!F?c zu5nIlsy55FN$i-kvPVr?2e@KZkC}mrZy&y$aV)v6=up(otY5Ycq&a6Jd_!XKqtBj@ zfnBW$`plP(K7X?JT2qE6?2ayKyW?AO)Rz#xAVy6YI+`~}U8cAM=^r$x{h(>1=R|-X zea+gx_8u=tDe2Xo%6;LL+AW^2jbmg=A9#I>LQsNkQ<@fKZSQAH$p7=*xqX`4eFGDY z89iZJ#?U9_aFp6r5&xz7_R^v*;gVNY`Q9{7m>o4m?UvRm|Dc8UDh5fF^a?$+IY)Dl z;rzv&!}{VSB6QRpRtma~$a20@zFWw~xHqtOd2lC z|Gn^?3Qt%mbnOU^NVb}E22+Z`U`a7rjLC*HOO`o1Co4JGXi730(+ptU5wf9v({-^=R(ZeEA^+k#R@ zJsFu}oeaEC6`%Wj`<2q9W7Z!x1RmaY=CQB&1ZejJX1tQttL~X~Hu%VH^I_k@{|pEA z#d*RC=mrd$NIc&nq_g18&m zX)kzr#d|-myk_e|O(QXJH)NPh*=CDbXEkJ*3_6|OnxwO4Sxwniqdv!&l4>@KUub!@ z)e*U?Y|V9!wbJ^hNfmwJIp07NAaAEZoSy{tkk0c3na}LEpT2NS=Y#W)#JHEM-A9BpXzrOVEQfua zT45iY>Dd4iy9)!;L{Uk2QC-zC!U^5oIsHSOEdrH$q$`Ze^}o?oF2I7$BsCXATe9sn zb&c~IY-?rdbDc>O(ZW_yefO&~pb0`NzUb^Qq|h)NpJqG0+{AoVU& Lgw>myXC3$-#ja~D diff --git a/Content/Levels/Main.umap b/Content/Levels/Main.umap index a25ece9ab07ef6588b4aeaa35ad2da2953a88b33..ff01d27f0417eaf9148788a076735b1c9e066c24 100644 GIT binary patch delta 152 zcmaF=M(zC@wGC#B0@F+6+c-Tke7D{b{iYnu%dpvv@x3RLvB_q0@8h}x&Wdhrd(X>z z^@i9M89s}1n_QS9GWl&K$7HR_+GI@g=&*s$=O&Oc>Cbs8IWCUU+AZ7+)79eH?Vm2UV2V#!xc@sGqRRML5Jm3HT delta 152 zcmaF=M(zC@wGC#B0=}uS+g0K=diGxuKK|m5@7>LAjPE^}jEpv$dmq;oSbK7|@Q&Gs zUH6+@mp!xKk>cdS9FfUyD>)`>RW29E-BVHKnD^DcPvgQh#c#$^n;%wQ5amdDyuf+M zIyUD?{Uwt(xbbXWJ<*i0Id5Wn-b6+qW&&bnAZ7t#Rv=~rVs;?r*q%3$lTj4_a;HQv diff --git a/Source/PBDRobotics/Link.cpp b/Source/PBDRobotics/Link.cpp index 0e4a5fa..73a2525 100644 --- a/Source/PBDRobotics/Link.cpp +++ b/Source/PBDRobotics/Link.cpp @@ -2,15 +2,18 @@ #include "Link.h" - -#include - #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(); diff --git a/Source/PBDRobotics/Link.h b/Source/PBDRobotics/Link.h index a4765bc..9bdcf58 100644 --- a/Source/PBDRobotics/Link.h +++ b/Source/PBDRobotics/Link.h @@ -51,9 +51,11 @@ public: UPROPERTY(EditAnywhere) bool IsEffector = false; + Vector3d GetGravityForce() const; +private: UPROPERTY(EditAnywhere) bool IsBase = false; -private: + Matrix3d Inertia_Tensor_Local; void SetupJoints() const; diff --git a/Source/PBDRobotics/Robot.cpp b/Source/PBDRobotics/Robot.cpp index 2d430ee..555867e 100644 --- a/Source/PBDRobotics/Robot.cpp +++ b/Source/PBDRobotics/Robot.cpp @@ -3,6 +3,8 @@ #include "Robot.h" +#include "util.h" + ARobot::ARobot(){ PrimaryActorTick.bCanEverTick = true; } @@ -17,12 +19,9 @@ void ARobot::BeginPlay(){ 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; + const Vector3d ConnectPosL1 = Joint->FirstLink->Position + Joint->GetFirstWorldDirection(); + Joint->SecondLink->Position = ConnectPosL1 - Joint->GetSecondWorldDirection(); + Joint->SecondLink->Orientation = Quaterniond::Identity(); } } @@ -45,11 +44,13 @@ void ARobot::Tick(const float DeltaTime){ const double h = DeltaTime / SubSteps; for (int i = 0; i < SubSteps; i++){ for (auto* Link : Links){ - Vector3d Gravity = Vector3d::Zero(); - if (HasGravity && !Link->IsBase) - Gravity = Vector3d(0, 0, -9.81 * 100) * Link->Mass; + Vector3d TransForce = Vector3d::Zero(); + if (HasGravity) + TransForce += Link->GetGravityForce(); + if (Link->IsEffector) + TransForce += Link->Mass * ToEigen(EffectorMove); const Vector3d RotForce = Vector3d::Zero(); - Link->Update(h, Gravity, RotForce); + Link->Update(h, TransForce, RotForce); } for (const auto* Joint : Joints) Joint->SolvePosition(h); diff --git a/Source/PBDRobotics/Robot.h b/Source/PBDRobotics/Robot.h index a5324a9..9abf5c9 100644 --- a/Source/PBDRobotics/Robot.h +++ b/Source/PBDRobotics/Robot.h @@ -29,7 +29,11 @@ protected: UPROPERTY(BlueprintReadWrite) bool HasGravity = false; + UPROPERTY(BlueprintReadWrite) + FVector EffectorMove = FVector::ZeroVector; + private: + // Assumes First Link to have Identity Orientation and Joints Array to be in order void ResetPositionAndOrientation(); public: virtual void Tick(float DeltaTime) override; diff --git a/Source/PBDRobotics/util.h b/Source/PBDRobotics/util.h index 54a4d42..a94dbe7 100644 --- a/Source/PBDRobotics/util.h +++ b/Source/PBDRobotics/util.h @@ -1,7 +1,6 @@ #pragma once #include "Eigen/Dense" -#include inline Vector3d ToEigen(const FVector V){ return Vector3d(V.X, V.Y, V.Z); @@ -11,27 +10,27 @@ inline Quaterniond ToEigen(const FQuat Q){ return Quaterniond(Q.W, Q.X, Q.Y, Q.Z); } -inline FVector FromEigen(const Vector3d V){ +inline FVector FromEigen(const Vector3d& V){ return FVector(V.x(), V.y(), V.z()); } -inline FQuat FromEigen(const Quaterniond Q){ +inline FQuat FromEigen(const Quaterniond& Q){ return FQuat(Q.x(), Q.y(), Q.z(), Q.w()); } -inline Quaterniond operator* (Quaterniond Q, double D){ +inline Quaterniond operator* (const Quaterniond& Q, const double D){ return Quaterniond(Q.w() * D, Q.x() * D, Q.y() * D, Q.z() * D); } -inline Quaterniond operator+ (const Quaterniond Q1, const Quaterniond Q2){ +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()); } -inline Quaterniond operator- (const Quaterniond Q1, const Quaterniond Q2){ +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()); } -inline void Log(MatrixXd M){ +inline void Log(const MatrixXd& M){ assert(M.rows() == 3); if (M.cols() == 3){ UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(0, 0), M(0, 1), M(0, 2));