From 9a5918fd1b3085059bd07400f8cd50148fd41213 Mon Sep 17 00:00:00 2001 From: Jb win Date: Thu, 1 May 2025 20:34:04 +0700 Subject: [PATCH] FIX - left side was not working + Removed useless var + added logs + cycle back to initial RestPosition --- .../Wdev/Robots/BP_SoArm100robot.uasset | Bin 112100 -> 112100 bytes .../RobotPilotSO100Component.cpp | 53 +++++++++++------- .../PilotComponent/RobotPilotSO100Component.h | 1 - 3 files changed, 32 insertions(+), 22 deletions(-) diff --git a/Content/Developers/Wdev/Robots/BP_SoArm100robot.uasset b/Content/Developers/Wdev/Robots/BP_SoArm100robot.uasset index b3a26b1b51ccd6ba0365f83c8289017abcfc814e..ffa0ac0e74fb616129e9fcaf6ba850d88708db69 100644 GIT binary patch delta 157 zcmaFzn(fJJwhhY}1^y?=DO|X`&u__<>X*)uHSafXXAFD6!f)i&uvzexnacJAKE`$y zMuzEmJd6_Cp9?X5j$u-9m|j@Is6TyOIivLS`FV^L)8`d19>x$pJ-wocu>eCDteRu` XmSV=;K)LNelX#e6inardG_wN$&f_~k delta 155 zcmaFzn(fJJwhhY}1$?(2b)Pb2nnOPaw_v-va`@)$jA1WWmcPht-7NUZOlA5dAx6>d z&U}pBEZd(8F@BC=`d~l3u!K>6`nqyP>FM+H7%QgFD_}f~A$)pzMG<2GhA>z)$Mh}5 VjJtty+kqzWFvApW2O4Q+2LOkoJ+J@( diff --git a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp index c5d800b7..f62fbf7c 100644 --- a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp +++ b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp @@ -133,9 +133,9 @@ void URobotPilotSO100Component::PostPhysicStepUpdate(const float SimulationTime) // Could be moved to if statement, but it becomes confusing - so let's keep a variable const bool bHasFinishedAnimation = AnimateActuators(SimulationTime); - if (bHasFinishedAnimation && CurrentAnimationState < MaxAnimationState) + if (bHasFinishedAnimation) { - AnimationStartTime = 0.; // Only for debug, can be left here but useless in normal operation mode + // AnimationStartTime = 0.; // Only for debug, can be left here but useless in normal operation mode NextAnimationState(); } } @@ -159,8 +159,6 @@ void URobotPilotSO100Component::NextAnimationState() return MoveToDropZone(); case 4: return OpenJaw(); - case 5: - return OpenJaw(); default: return BasePose(); } @@ -168,14 +166,23 @@ void URobotPilotSO100Component::NextAnimationState() void URobotPilotSO100Component::BasePose() { + UE_LOG(LogTemp, Log, TEXT("Animate -> BasePose")); CurrentAnimationState = 0; - AnimationDuration = 1.f; + AnimationDuration = 1.5f; AnimTargetRobotActuators = ActuatorsRestPosition; - // AnimTargetRobotActuators = ActuatorsMaxExtendPosition; } void URobotPilotSO100Component::RotateToTarget() { + UE_LOG(LogTemp, Log, TEXT("Animate -> RotateToTarget")); + + if (TargetTransform.GetLocation() == FVector::ZeroVector) + { + AnimationStartTime = 0.f; + CurrentAnimationState = 0; + return; + } + // Compute Pivot Point World Location const auto WorldTransform = RobotOwner->RobotActor->GetActorTransform(); const FVector PivotWorldLocation = WorldTransform.GetLocation() + WorldTransform.GetRotation().RotateVector(PivotOffset); @@ -186,10 +193,12 @@ void URobotPilotSO100Component::RotateToTarget() TargetTransform.GetLocation() ); + // reduce/increase Yaw to not have the fixed jaw colliding with the shape - TODO use middle of the jaw instead of the wall of the jaw + const auto Dot = FVector::DotProduct(RotationToTarget.Quaternion().GetForwardVector(), WorldTransform.GetRotation().GetForwardVector()); + const auto Mod = .1 * (Dot > 0 ? 1 : -1); + // Convert to radians - // Looks like we are not in the same referential hence the -1 ! - // reduce Yaw to not have the fixed jaw colliding with the shape - const auto ActuatorRotation = RotationToTarget.Yaw * 0.9 / 180.0f * -PI; + const auto ActuatorRotation = RotationToTarget.Yaw * (1+Mod) / 180.0f * -PI; // Looks like we are not in the same referential hence the -PI instead of PI ! // Start the animation AnimTargetRobotActuators = AnimStartRobotActuators; @@ -200,6 +209,8 @@ void URobotPilotSO100Component::RotateToTarget() void URobotPilotSO100Component::MoveToTarget() { + UE_LOG(LogTemp, Log, TEXT("Animate -> MoveToTarget")); + // Get Pivot World const auto WorldTransform = RobotOwner->RobotActor->GetActorTransform(); const FVector PivotWorldLocation = WorldTransform.GetLocation() + WorldTransform.GetRotation().RotateVector(PivotOffset); @@ -212,16 +223,7 @@ void URobotPilotSO100Component::MoveToTarget() const auto JawPositionWorld = RotationToTarget.RotateVector(JawOffsetToPivot) + PivotWorldLocation; const auto Distance = FVector::Distance(JawPositionWorld, TargetTransform.GetLocation()); - DrawDebugLine( - this->GetWorld(), - JawPositionWorld, - JawPositionWorld + FVector::UpVector * 100, - FColor::Blue, - true - ); - const auto AlphaExtend = FMath::Clamp(Distance / MaxReach, 0., 1.); - // UE_LOG(LogTemp, Log, TEXT("Distance: %f - AlphaExtend: %f"), Distance, AlphaExtend); // Set the target actuators values AnimTargetRobotActuators = GetCurrentJointsFromPhysicScene(); @@ -233,7 +235,7 @@ void URobotPilotSO100Component::MoveToTarget() // Start the animation CurrentAnimationState = 2; - AnimationDuration = 1.f; + AnimationDuration = 2.f; DrawDebugLine( this->GetWorld(), @@ -246,17 +248,24 @@ void URobotPilotSO100Component::MoveToTarget() void URobotPilotSO100Component::CloseJaw() { + UE_LOG(LogTemp, Log, TEXT("Animate -> CloseJaw")); + // Here we need overcurrent detection - not here actually AnimTargetRobotActuators.Jaw = ClosedJaw; + // Reset TargetTransform + TargetTransform = FTransform(); + // Start the animation bDetectOverCurrent = true; CurrentAnimationState = 3; - AnimationDuration = 1.6f; + AnimationDuration = 2.f; } void URobotPilotSO100Component::MoveToDropZone() { + UE_LOG(LogTemp, Log, TEXT("Animate -> MoveToDropZone")); + AnimTargetRobotActuators = ActuatorsDropZone; AnimTargetRobotActuators.Rotation = ActuatorsDropZone.Rotation * (FMath::RandBool() ? 1. : -1.); AnimTargetRobotActuators.Jaw = GetCurrentJointsFromPhysicScene().Jaw; @@ -266,6 +275,8 @@ void URobotPilotSO100Component::MoveToDropZone() void URobotPilotSO100Component::OpenJaw() { + UE_LOG(LogTemp, Log, TEXT("Animate -> OpenJaw")); + AnimTargetRobotActuators.Jaw = OpenedJaw; CurrentAnimationState = 5; AnimationDuration = 0.6f; @@ -286,7 +297,7 @@ bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime) } - // UE_LOG(LogTemp, Log, TEXT("AnimationAlpha: %f - Delta: %f"), AnimAlpha, Delta); + // UE_LOG(LogTemp, Log, TEXT("AnimationAlpha: %f - Delta: %f"), AnimAlpha, DeltaSum); // Stop the animation if we reached the target if (AnimAlpha >= 1. && DeltaSum <= .001) return true; diff --git a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h index 5b5f17d9..bc1db666 100644 --- a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h +++ b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h @@ -104,7 +104,6 @@ private: // 4 -> Go to drop zone // 5 -> open jaw int32 CurrentAnimationState = -1; - int32 MaxAnimationState = 5; void NextAnimationState(); void BasePose();