diff --git a/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset b/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset index 9175c3b7..68616f5b 100644 Binary files a/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset and b/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset differ diff --git a/Content/Developers/Wdev/Robots/BP_SoArm100robot.uasset b/Content/Developers/Wdev/Robots/BP_SoArm100robot.uasset index 61e79171..b3a26b1b 100644 Binary files a/Content/Developers/Wdev/Robots/BP_SoArm100robot.uasset and b/Content/Developers/Wdev/Robots/BP_SoArm100robot.uasset differ diff --git a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp index 66b7edb9..c5d800b7 100644 --- a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp +++ b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp @@ -4,10 +4,12 @@ #include "Kismet/KismetMathLibrary.h" #include "Kismet/KismetSystemLibrary.h" #include "Robot/RobotPawn.h" +#include "Serialization/ShaderKeyGenerator.h" URobotPilotSO100Component::URobotPilotSO100Component() { - + PrimaryComponentTick.bCanEverTick = true; + PrimaryComponentTick.bStartWithTickEnabled = true; } void URobotPilotSO100Component::BeginPlay() @@ -18,20 +20,30 @@ void URobotPilotSO100Component::BeginPlay() void URobotPilotSO100Component::TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { - Super::TickComponent(DeltaTime, TickType, ThisTickFunction); + // Super::TickComponent(DeltaTime, TickType, ThisTickFunction); + // const auto Joints = GetCurrentJointsFromPhysicScene(); + // const auto Controls = GetCurrentControlsFromPhysicScene(); + // UE_LOG(LogTemp, Log, TEXT("Joint: %f - Control: %f - Delta: %f"), Joints.Jaw, Controls.Jaw, Controls.Jaw - Joints.Jaw); } + void URobotPilotSO100Component::SetTarget(const FTransform& TargetTransformIn) { // Set Base Values - AnimStartRobotActuators = GetCurrentActuatorsFromPhysicScene(); TargetTransform = TargetTransformIn; NextAnimationState(); } -void URobotPilotSO100Component::PrintActuators() const + + +void URobotPilotSO100Component::PrintCurrentActuators() const { - const auto [Rotation, Pitch, Elbow, WristPitch, WristRoll, Jaw] = GetCurrentActuatorsFromPhysicScene(); + PrintActuators(GetCurrentControlsFromPhysicScene()); +} + +void URobotPilotSO100Component::PrintActuators(FSo100Actuators Actuators) +{ + const auto [Rotation, Pitch, Elbow, WristPitch, WristRoll, Jaw] = Actuators; UE_LOG(LogTemp, Display, TEXT("Actuators Rotation %f | Pitch %f | Elbow %f | WristPitch %f | WristRoll %f | Jaw %f"), Rotation, Pitch, @@ -42,16 +54,73 @@ void URobotPilotSO100Component::PrintActuators() const ); } -FSo100Actuators URobotPilotSO100Component::GetCurrentActuatorsFromPhysicScene() const +void URobotPilotSO100Component::DisableAnim() +{ + AnimationStartTime = 0; +} + +FSo100Actuators URobotPilotSO100Component::GetCurrentControlsFromPhysicScene() const { return FSo100Actuators { - RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Rotation), - RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Pitch), - RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Elbow), - RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_WristPitch), - RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_WristRoll), - RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Jaw), + RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Rotation), + RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Pitch), + RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Elbow), + RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_WristPitch), + RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_WristRoll), + RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Jaw), + }; +} + +FSo100Actuators URobotPilotSO100Component::GetCurrentJointsFromPhysicScene() const +{ + // Due to Mujoco import plugin not renaming joint differently from controller and Unreal refusing to have 2 variables with the same name + // Joints get a "1" appended to their name -> refacto so it's "_Joint" + return FSo100Actuators + { + RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Rotation).Append("1")), + RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Pitch).Append("1")), + RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Elbow).Append("1")), + RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_WristPitch).Append("1")), + RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_WristRoll).Append("1")), + RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Jaw).Append("1")) + }; +} + +float URobotPilotSO100Component::GetDeltaSumBetweenActuatorValues(const FSo100Actuators& A, const FSo100Actuators& B) +{ + float DeltaSum = 0; + + DeltaSum += FMath::Abs(A.Rotation) - FMath::Abs(B.Rotation); + DeltaSum += FMath::Abs(A.Pitch) - FMath::Abs(B.Pitch); + DeltaSum += FMath::Abs(A.Elbow) - FMath::Abs(B.Elbow); + DeltaSum += FMath::Abs(A.WristPitch) - FMath::Abs(B.WristPitch); + DeltaSum += FMath::Abs(A.WristRoll) - FMath::Abs(B.WristRoll); + DeltaSum += FMath::Abs(A.Jaw) - FMath::Abs(B.Jaw); + + return DeltaSum; +} + +double URobotPilotSO100Component::GetControlJointDeltaForActuator(FString ActuatorName) const +{ + auto const Control = RobotOwner->PhysicSceneProxy->GetActuatorValue(ActuatorName); + auto const Joint = RobotOwner->PhysicSceneProxy->GetJointValue(ActuatorName.Append("1")); + return Control -Joint; +} + +FSo100Actuators URobotPilotSO100Component::LerpActuators( + const FSo100Actuators& A, + const FSo100Actuators& B, + const float Alpha + ) +{ + return FSo100Actuators{ + FMath::Lerp( A.Rotation, B.Rotation, Alpha), + FMath::Lerp( A.Pitch, B.Pitch, Alpha), + FMath::Lerp( A.Elbow, B.Elbow, Alpha), + FMath::Lerp( A.WristPitch, B.WristPitch, Alpha), + FMath::Lerp( A.WristRoll, B.WristRoll, Alpha), + FMath::Lerp( A.Jaw, B.Jaw, Alpha), }; } @@ -59,13 +128,14 @@ void URobotPilotSO100Component::PostPhysicStepUpdate(const float SimulationTime) { // Do anything that should be done after a mj_step update - if (AnimationStartTime >= 0) + if (AnimationStartTime > 0) { // 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) { + AnimationStartTime = 0.; // Only for debug, can be left here but useless in normal operation mode NextAnimationState(); } } @@ -75,19 +145,22 @@ void URobotPilotSO100Component::NextAnimationState() { const float CurrentSimulationTime = RobotOwner->PhysicSceneProxy->GetMujocoData().time; AnimationStartTime = CurrentSimulationTime; + AnimStartRobotActuators = GetCurrentJointsFromPhysicScene(); switch (CurrentAnimationState) { case 0: return RotateToTarget(); case 1: - return CloseJaw(); + return MoveToTarget(); case 2: - return MoveToDropZone(); + return CloseJaw(); case 3: - return OpenJaw(); + return MoveToDropZone(); case 4: return OpenJaw(); + case 5: + return OpenJaw(); default: return BasePose(); } @@ -96,98 +169,165 @@ void URobotPilotSO100Component::NextAnimationState() void URobotPilotSO100Component::BasePose() { CurrentAnimationState = 0; - AnimationDuration = 1.5f; + AnimationDuration = 1.f; AnimTargetRobotActuators = ActuatorsRestPosition; + // AnimTargetRobotActuators = ActuatorsMaxExtendPosition; } void URobotPilotSO100Component::RotateToTarget() { - CurrentAnimationState = 1; - AnimationDuration = 1.f; - // Compute Pivot Point World Location const auto WorldTransform = RobotOwner->RobotActor->GetActorTransform(); - const FVector PivotWorldLocation = WorldTransform.GetTranslation() + WorldTransform.GetRotation().RotateVector(PivotOffset); - + const FVector PivotWorldLocation = WorldTransform.GetLocation() + WorldTransform.GetRotation().RotateVector(PivotOffset); - // FVector::CosineAngle2D() - const auto Angle = UKismetMathLibrary::FindLookAtRotation( - WorldTransform.GetRotation().GetForwardVector(), - TargetTransform.GetLocation() - PivotWorldLocation + // Find Yaw Rotation in degree - cache it for distance to target computation + RotationToTarget = UKismetMathLibrary::FindLookAtRotation( + PivotWorldLocation, + TargetTransform.GetLocation() ); - - const auto Cosine = WorldTransform.GetRotation().GetForwardVector().CosineAngle2D(TargetTransform.GetLocation() - PivotWorldLocation); - UE_LOG(LogTemp, Display, TEXT("Rotation %f, %f, %f"), Angle.Roll, Angle.Pitch, Angle.Yaw); - UE_LOG(LogTemp, Display, TEXT("Cosine %f"), Cosine); + // 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; + + // Start the animation + AnimTargetRobotActuators = AnimStartRobotActuators; + AnimTargetRobotActuators.Rotation = ActuatorRotation; + CurrentAnimationState = 1; + AnimationDuration = .7f; +} + +void URobotPilotSO100Component::MoveToTarget() +{ + // Get Pivot World + const auto WorldTransform = RobotOwner->RobotActor->GetActorTransform(); + const FVector PivotWorldLocation = WorldTransform.GetLocation() + WorldTransform.GetRotation().RotateVector(PivotOffset); + + // Rotate Jaw offset towards target + // Get pure 2d rotation + RotationToTarget.Pitch = 0; + RotationToTarget.Roll = 0; + const auto JawOffsetToPivot = JawOffset - PivotOffset; + 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(); + AnimTargetRobotActuators = LerpActuators(ActuatorsRestPosition, ActuatorsMaxExtendPosition, AlphaExtend); + + + AnimTargetRobotActuators.Rotation = AnimStartRobotActuators.Rotation; + PrintActuators(AnimTargetRobotActuators); + + // Start the animation + CurrentAnimationState = 2; + AnimationDuration = 1.f; DrawDebugLine( this->GetWorld(), - PivotWorldLocation + 100 * FVector::UpVector, - PivotWorldLocation, - FColor::Blue, - true); + JawPositionWorld, + JawPositionWorld + (TargetTransform.GetLocation() - JawPositionWorld).GetSafeNormal() * Distance, + FColor::Green, + true + ); } void URobotPilotSO100Component::CloseJaw() { // Here we need overcurrent detection - not here actually + AnimTargetRobotActuators.Jaw = ClosedJaw; + + // Start the animation + bDetectOverCurrent = true; + CurrentAnimationState = 3; + AnimationDuration = 1.6f; } void URobotPilotSO100Component::MoveToDropZone() { - const auto DropZoneActuatorsSettings = FMath::RandBool() ? ActuatorsLeftDropZone : ActuatorsRightDropZone; - + AnimTargetRobotActuators = ActuatorsDropZone; + AnimTargetRobotActuators.Rotation = ActuatorsDropZone.Rotation * (FMath::RandBool() ? 1. : -1.); + AnimTargetRobotActuators.Jaw = GetCurrentJointsFromPhysicScene().Jaw; + CurrentAnimationState = 4; + AnimationDuration = 3.f; } void URobotPilotSO100Component::OpenJaw() { + AnimTargetRobotActuators.Jaw = OpenedJaw; + CurrentAnimationState = 5; + AnimationDuration = 0.6f; } -bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime) const +bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime) { const double AnimAlpha = FMath::Clamp((SimulationTime - AnimationStartTime) / AnimationDuration, 0., 1.); + // Need to wait for the joints to be in the right position before switching to next animation + const auto DeltaSum = GetDeltaSumBetweenActuatorValues(AnimTargetRobotActuators, GetCurrentJointsFromPhysicScene()); + + // Alternative Animation completion event - checking for over-current + if (bDetectOverCurrent && GetControlJointDeltaForActuator(Actuator_Jaw) > OverCurrentThreshold) + { + bDetectOverCurrent = false; + return true; + } + + + // UE_LOG(LogTemp, Log, TEXT("AnimationAlpha: %f - Delta: %f"), AnimAlpha, Delta); + // Stop the animation if we reached the target - if (AnimAlpha >= 1.) return true; + if (AnimAlpha >= 1. && DeltaSum <= .001) return true; // Rotation - RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Rotation, FMath::Lerp( + RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Rotation, FMath::Lerp( AnimStartRobotActuators.Rotation, AnimTargetRobotActuators.Rotation, AnimAlpha )); // Pitch - RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Pitch, FMath::Lerp( + RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Pitch, FMath::Lerp( AnimStartRobotActuators.Pitch, AnimTargetRobotActuators.Pitch, AnimAlpha )); // Elbow - RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Elbow, FMath::Lerp( + RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Elbow, FMath::Lerp( AnimStartRobotActuators.Elbow, AnimTargetRobotActuators.Elbow, AnimAlpha )); // WristPitch - RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_WristPitch, FMath::Lerp( + RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_WristPitch, FMath::Lerp( AnimStartRobotActuators.WristPitch, AnimTargetRobotActuators.WristPitch, AnimAlpha )); // WristRoll - RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_WristRoll, FMath::Lerp( + RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_WristRoll, FMath::Lerp( AnimStartRobotActuators.WristRoll, AnimTargetRobotActuators.WristRoll, AnimAlpha )); // Jaw - RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Jaw, FMath::Lerp( + RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Jaw, FMath::Lerp( AnimStartRobotActuators.Jaw, AnimTargetRobotActuators.Jaw, AnimAlpha diff --git a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h index 532e6915..5b5f17d9 100644 --- a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h +++ b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h @@ -41,40 +41,64 @@ public: void SetTarget(const FTransform& TargetTransformIn); private: FTransform TargetTransform; - + + //--------------------- + //------- DEBUG ------- + //--------------------- public: UFUNCTION(BlueprintCallable) - void PrintActuators() const; + void PrintCurrentActuators() const; + static void PrintActuators(FSo100Actuators Actuators); + UFUNCTION(BlueprintCallable) + void DisableAnim(); + private: // SO100 Controls by name - FString Ctrl_Rotation = FString("Rotation"); - FString Ctrl_Pitch = FString("Pitch"); - FString Ctrl_Elbow = FString("Elbow"); - FString Ctrl_WristPitch = FString("Wrist_Pitch"); - FString Ctrl_WristRoll = FString("Wrist_Roll"); - FString Ctrl_Jaw = FString("Jaw"); + FString Actuator_Rotation = FString("Rotation"); + FString Actuator_Pitch = FString("Pitch"); + FString Actuator_Elbow = FString("Elbow"); + FString Actuator_WristPitch = FString("Wrist_Pitch"); + FString Actuator_WristRoll = FString("Wrist_Roll"); + FString Actuator_Jaw = FString("Jaw"); // SO100 Static Variables FVector PivotOffset = FVector{-0.000030, 4.520021, 1.650041}; + FVector JawOffset = FVector{23, 2, 9}; + float MaxReach = 20.757929; // fixed_jaw_pad_3 ForwardVectorLength Delta between Rest and MaxExtend + + // Actuators Joints and Controls are expressed in doubles + double ClosedJaw = 0.18; + double OpenedJaw = -2.0; /** * Query the physic proxy on the RobotOwner to get the SO100 actuators values * @return */ - FSo100Actuators GetCurrentActuatorsFromPhysicScene() const; + FSo100Actuators GetCurrentControlsFromPhysicScene() const; + FSo100Actuators GetCurrentJointsFromPhysicScene() const; + double GetControlJointDeltaForActuator(FString ActuatorName) const; + static float GetDeltaSumBetweenActuatorValues(const FSo100Actuators& A, const FSo100Actuators& B); + static FSo100Actuators LerpActuators(const FSo100Actuators& A, const FSo100Actuators& B, const float Alpha); // Called after every physic step virtual void PostPhysicStepUpdate(const float SimulationTime) override; - bool AnimateActuators(float SimulationTime) const; // Bound to the PhysicProxy post-update delegate + bool AnimateActuators(float SimulationTime); // Bound to the PhysicProxy post-update delegate FSo100Actuators CurrentRobotActuators; // This will be updated by the post-physic delegate FSo100Actuators AnimStartRobotActuators; FSo100Actuators AnimTargetRobotActuators; + FRotator RotationToTarget = FRotator::ZeroRotator; + + // ------------------------ + // ----- OVER-CURRENT ----- + // ------------------------ + bool bDetectOverCurrent = false; + const float OverCurrentThreshold = 0.15; // Quick and dirty sequence of moves // -1 -> Start Game, extended // 0 -> Retract - base pose - // 1 -> Rotate towards goal + // 1 -> Rotate towards target // 2 -> Move to target // 3 -> Close the jaw // 4 -> Go to drop zone @@ -85,6 +109,7 @@ private: void BasePose(); void RotateToTarget(); + void MoveToTarget(); void CloseJaw(); void MoveToDropZone(); void OpenJaw(); @@ -104,32 +129,23 @@ private: }; FSo100Actuators ActuatorsMaxExtendPosition { - 0., - 0., - 0., - 0., - -1.56, - -1.045 + 0.0000001, + 0.081027, + -0.01707, + -0.075, + 1.469020, + -1.389073 }; - FSo100Actuators ActuatorsLeftDropZone { - 0., - 0., - 0., - 0., - -1.56, - -1.045 + FSo100Actuators ActuatorsDropZone { + PI / 2, + -2.17, + 0.805, + 1.345, + 1.61, + 0 }; - FSo100Actuators ActuatorsRightDropZone { - 0., - 0., - 0., - 0., - -1.56, - -1.045 - }; - // Tick where it can have targets // Open Claw (ClawIndex) // Presets to move certain joints into certain positions -> HardCode