diff --git a/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset b/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset index a11657d3..d9239fe8 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..ffa0ac0e 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/RobotPilotComponent.cpp b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp new file mode 100644 index 00000000..173d3bab --- /dev/null +++ b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp @@ -0,0 +1,37 @@ +#include "Robot/PilotComponent/RobotPilotComponent.h" + +#include "Actors/MujocoVolumeActor.h" +#include "Robot/RobotPawn.h" + +URobotPilotComponent::URobotPilotComponent() +{ + +} + +void URobotPilotComponent::BeginPlay() +{ + Super::BeginPlay(); + + // Reference owning robot + RobotOwner = Cast(GetOwner()); + InitPilotComponent(); +} + +void URobotPilotComponent::TickComponent(float DeltaTime, enum ELevelTick TickType, + FActorComponentTickFunction* ThisTickFunction) +{ + Super::TickComponent(DeltaTime, TickType, ThisTickFunction); +} + +void URobotPilotComponent::InitPilotComponent() +{ + if (RobotOwner.Get() && RobotOwner->PhysicsSceneProxy.Get()) + { + RobotOwner->PhysicsSceneProxy->BindPostPhysicStepDelegate(this, &URobotPilotComponent::PostPhysicStepUpdate); + } +} + +void URobotPilotComponent::PostPhysicStepUpdate(const float SimulationTime) +{ + // Overriden in each dedicated RobotPilotComponent +} diff --git a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotMultiRotorDrone.cpp b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotMultiRotorDrone.cpp new file mode 100644 index 00000000..78af2c80 --- /dev/null +++ b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotMultiRotorDrone.cpp @@ -0,0 +1,5 @@ +#include "Robot/PilotComponent/RobotPilotMultiRotorDrone.h" + +URobotPilotMultiRotorDrone::URobotPilotMultiRotorDrone() +{ +} diff --git a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp new file mode 100644 index 00000000..dc1357ca --- /dev/null +++ b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp @@ -0,0 +1,363 @@ +#include "Robot/PilotComponent/RobotPilotSO100Component.h" + +#include "Actors/MujocoVolumeActor.h" +#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() +{ + Super::BeginPlay(); +} + +void URobotPilotSO100Component::TickComponent(float DeltaTime, enum ELevelTick TickType, + FActorComponentTickFunction* ThisTickFunction) +{ + // Super::TickComponent(DeltaTime, TickType, ThisTickFunction); + // const auto Joints = GetCurrentJointsFromPhysicsScene(); + // 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 + TargetTransform = TargetTransformIn; + NextAnimationState(); +} + + + +void URobotPilotSO100Component::PrintCurrentActuators() const +{ + 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, + Elbow, + WristPitch, + WristRoll, + Jaw + ); +} + +void URobotPilotSO100Component::DisableAnim() +{ + AnimationStartTime = 0; +} + +void URobotPilotSO100Component::DebugUpdatePos() +{ + const int JointId = RobotOwner->PhysicsSceneProxy->GetJointId("MujocoBody_6"); + auto MjData = RobotOwner->PhysicsSceneProxy->GetMujocoData(); + auto MjModel = RobotOwner->PhysicsSceneProxy->GetMujocoModel(); + // mj_name2id(MjModel, mjOBJ_JOINT, ""); + + if (JointId) + { + auto adx = MjModel.jnt_qposadr[JointId]; + mjtNum* body_pos = MjData.xpos + 3 * JointId; + UE_LOG(LogTemp, Log, TEXT("JointId %i - pos.x %f - pos.y %f - pos.z %f"), JointId, body_pos[0], body_pos[1], body_pos[2]); + } +} + +FSo100Actuators URobotPilotSO100Component::GetCurrentControlsFromPhysicScene() const +{ + return FSo100Actuators + { + RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Rotation), + RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Pitch), + RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Elbow), + RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_WristPitch), + RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_WristRoll), + RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Jaw), + }; +} + +FSo100Actuators URobotPilotSO100Component::GetCurrentJointsFromPhysicsScene() 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->PhysicsSceneProxy->GetJointValue(FString(Actuator_Rotation).Append("1")), + RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Pitch).Append("1")), + RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Elbow).Append("1")), + RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_WristPitch).Append("1")), + RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_WristRoll).Append("1")), + RobotOwner->PhysicsSceneProxy->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->PhysicsSceneProxy->GetActuatorValue(ActuatorName); + auto const Joint = RobotOwner->PhysicsSceneProxy->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), + }; +} + +void URobotPilotSO100Component::PostPhysicStepUpdate(const float SimulationTime) +{ + // Do anything that should be done after a mj_step update + + 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) + { + // AnimationStartTime = 0.; // Only for debug, can be left here but useless in normal operation mode + NextAnimationState(); + } + } +} + +void URobotPilotSO100Component::NextAnimationState() +{ + const float CurrentPhysicsEngineSceneTime = RobotOwner->PhysicsSceneProxy->GetMujocoData().time; + AnimationStartTime = CurrentPhysicsEngineSceneTime; + AnimStartRobotActuators = GetCurrentJointsFromPhysicsScene(); + + switch (CurrentAnimationState) + { + case 0: + return RotateToTarget(); + case 1: + return MoveToTarget(); + case 2: + return CloseJaw(); + case 3: + return MoveToDropZone(); + case 4: + return OpenJaw(); + default: + return BasePose(); + } +} + +void URobotPilotSO100Component::BasePose() +{ + UE_LOG(LogTemp, Log, TEXT("Animate -> BasePose")); + CurrentAnimationState = 0; + AnimationDuration = 1.5f; + AnimTargetRobotActuators = ActuatorsRestPosition; +} + +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); + + // Find Yaw Rotation in degree - cache it for distance to target computation + RotationToTarget = UKismetMathLibrary::FindLookAtRotation( + PivotWorldLocation, + 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 + 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; + AnimTargetRobotActuators.Rotation = ActuatorRotation; + CurrentAnimationState = 1; + AnimationDuration = .7f; +} + +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); + + // 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()); + + const auto AlphaExtend = FMath::Clamp(Distance / MaxReach, 0., 1.); + + // Set the target actuators values + AnimTargetRobotActuators = GetCurrentJointsFromPhysicsScene(); + AnimTargetRobotActuators = LerpActuators(ActuatorsRestPosition, ActuatorsMaxExtendPosition, AlphaExtend); + + + AnimTargetRobotActuators.Rotation = AnimStartRobotActuators.Rotation; + PrintActuators(AnimTargetRobotActuators); + + // Start the animation + CurrentAnimationState = 2; + AnimationDuration = 2.f; + + DrawDebugLine( + this->GetWorld(), + JawPositionWorld, + JawPositionWorld + (TargetTransform.GetLocation() - JawPositionWorld).GetSafeNormal() * Distance, + FColor::Green, + true + ); +} + +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 = 2.f; +} + +void URobotPilotSO100Component::MoveToDropZone() +{ + UE_LOG(LogTemp, Log, TEXT("Animate -> MoveToDropZone")); + + AnimTargetRobotActuators = ActuatorsDropZone; + AnimTargetRobotActuators.Rotation = ActuatorsDropZone.Rotation * (FMath::RandBool() ? 1. : -1.); + AnimTargetRobotActuators.Jaw = GetCurrentJointsFromPhysicsScene().Jaw; + CurrentAnimationState = 4; + AnimationDuration = 3.f; +} + +void URobotPilotSO100Component::OpenJaw() +{ + UE_LOG(LogTemp, Log, TEXT("Animate -> OpenJaw")); + + AnimTargetRobotActuators.Jaw = OpenedJaw; + CurrentAnimationState = 5; + AnimationDuration = 0.6f; +} + +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, GetCurrentJointsFromPhysicsScene()); + + // 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, DeltaSum); + + // Stop the animation if we reached the target + if (AnimAlpha >= 1. && DeltaSum <= .001) return true; + + // Rotation + RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Rotation, FMath::Lerp( + AnimStartRobotActuators.Rotation, + AnimTargetRobotActuators.Rotation, + AnimAlpha + )); + + // Pitch + RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Pitch, FMath::Lerp( + AnimStartRobotActuators.Pitch, + AnimTargetRobotActuators.Pitch, + AnimAlpha + )); + + // Elbow + RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Elbow, FMath::Lerp( + AnimStartRobotActuators.Elbow, + AnimTargetRobotActuators.Elbow, + AnimAlpha + )); + + // WristPitch + RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_WristPitch, FMath::Lerp( + AnimStartRobotActuators.WristPitch, + AnimTargetRobotActuators.WristPitch, + AnimAlpha + )); + + // WristRoll + RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_WristRoll, FMath::Lerp( + AnimStartRobotActuators.WristRoll, + AnimTargetRobotActuators.WristRoll, + AnimAlpha + )); + + // Jaw + RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Jaw, FMath::Lerp( + AnimStartRobotActuators.Jaw, + AnimTargetRobotActuators.Jaw, + AnimAlpha + )); + + return false; +} diff --git a/Source/LuckyWorldV2/Private/Robot/RobotPawn.cpp b/Source/LuckyWorldV2/Private/Robot/RobotPawn.cpp new file mode 100644 index 00000000..2bbe3402 --- /dev/null +++ b/Source/LuckyWorldV2/Private/Robot/RobotPawn.cpp @@ -0,0 +1,67 @@ +#include "Robot/RobotPawn.h" + +#include "Robot/PilotComponent/RobotPilotMultiRotorDrone.h" +#include "Robot/PilotComponent/RobotPilotSO100Component.h" + +ARobotPawn::ARobotPawn() +{ +} + +void ARobotPawn::BeginPlay() +{ + Super::BeginPlay(); + InitRobot(); // TODO Maybe move to GameInstance to control when we initialize the robot completely +} + +void ARobotPawn::InitRobot() +{ + InitPilotComponent(); + // Other initialization tasks +} + +void ARobotPawn::InitPilotComponent() +{ + // Initialize pilot component based on robot type + switch (RobotType) + { + case ERobotsName::None: + break; + + case ERobotsName::SO100Robot: + RobotPilotComponent = NewObject(this); + break; + + case ERobotsName::DJIDrone: + RobotPilotComponent = NewObject(this); + break; + + case ERobotsName::Luck_e: + break; // TODO or remove from enum + case ERobotsName::Stretch: + break; // TODO or remove from enum + case ERobotsName::LuckyDrone: + break; // TODO or remove from enum + case ERobotsName::ArmLucky: + break; // TODO or remove from enum + case ERobotsName::UnitreeG1: + break; // TODO or remove from enum + case ERobotsName::StretchRobotV1: + break; // TODO or remove from enum + case ERobotsName::PandaArmRobot: + break; // TODO or remove from enum + case ERobotsName::PuralinkRobot: + break; // TODO or remove from enum + case ERobotsName::UnitreeGo2: + break; // TODO or remove from enum + case ERobotsName::RevoluteRobot: + break; // TODO or remove from enum + case ERobotsName::BostonSpotRobot: + break; // TODO or remove from enum + } + + // Register if this Robot has a Pilot Component + if (RobotPilotComponent) + { + RobotPilotComponent->RegisterComponent(); + } +} diff --git a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotComponent.h b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotComponent.h new file mode 100644 index 00000000..12a1c4eb --- /dev/null +++ b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotComponent.h @@ -0,0 +1,40 @@ +#pragma once +#include "CoreMinimal.h" +#include "RobotPilotComponent.generated.h" + +USTRUCT(BlueprintType) +struct FRobotActuators +{ + GENERATED_BODY() + // Do we need a prent struct? + // What will be in common? +}; + +class ARobotPawn; + +UCLASS(Blueprintable) +class LUCKYWORLDV2_API URobotPilotComponent : public UActorComponent + + +{ + GENERATED_BODY() + +public: + URobotPilotComponent(); + + virtual void BeginPlay() override; + virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + virtual void InitPilotComponent(); + virtual void PostPhysicStepUpdate(const float SimulationTime); + +protected: // Child class need access + // Only to easy access within the component + TObjectPtr RobotOwner = nullptr; + + // ---------------- + // ----- ANIM ----- + // ---------------- +protected: // Child class need access + float AnimationStartTime = 0.f; + float AnimationDuration = 0.f; +}; diff --git a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotMultiRotorDrone.h b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotMultiRotorDrone.h new file mode 100644 index 00000000..3452ea19 --- /dev/null +++ b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotMultiRotorDrone.h @@ -0,0 +1,13 @@ +#pragma once +#include "CoreMinimal.h" +#include "Robot/PilotComponent/RobotPilotComponent.h" +#include "RobotPilotMultiRotorDrone.generated.h" + +UCLASS(Blueprintable) +class LUCKYWORLDV2_API URobotPilotMultiRotorDrone : public URobotPilotComponent +{ + GENERATED_BODY() + +public: + URobotPilotMultiRotorDrone(); +}; diff --git a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h new file mode 100644 index 00000000..6e325e23 --- /dev/null +++ b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h @@ -0,0 +1,152 @@ +#pragma once +#include "CoreMinimal.h" +#include "Robot/PilotComponent/RobotPilotComponent.h" +#include "RobotPilotSO100Component.generated.h" + +USTRUCT(BlueprintType) +struct FSo100Actuators +{ + GENERATED_BODY() + UPROPERTY(EditAnywhere, BlueprintReadWrite) + double Rotation = 0.; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + double Pitch = 0.; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + double Elbow = 0.; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + double WristPitch = 0.; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + double WristRoll = 0.; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + double Jaw = 0.; +}; + +UCLASS(Blueprintable) +class LUCKYWORLDV2_API URobotPilotSO100Component : public URobotPilotComponent +{ + GENERATED_BODY() + +public: + URobotPilotSO100Component(); + + virtual void BeginPlay() override; + virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + + UFUNCTION(BlueprintCallable) + void SetTarget(const FTransform& TargetTransformIn); +private: + FTransform TargetTransform; + + //--------------------- + //------- DEBUG ------- + //--------------------- +public: + UFUNCTION(BlueprintCallable) + void PrintCurrentActuators() const; + static void PrintActuators(FSo100Actuators Actuators); + + UFUNCTION(BlueprintCallable) + void DisableAnim(); + + UFUNCTION(BlueprintCallable) + void DebugUpdatePos(); + +private: + // SO100 Controls by name + 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 GetCurrentControlsFromPhysicScene() const; + FSo100Actuators GetCurrentJointsFromPhysicsScene() 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); // 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 target + // 2 -> Move to target + // 3 -> Close the jaw + // 4 -> Go to drop zone + // 5 -> open jaw + int32 CurrentAnimationState = -1; + void NextAnimationState(); + + void BasePose(); + void RotateToTarget(); + void MoveToTarget(); + void CloseJaw(); + void MoveToDropZone(); + void OpenJaw(); + + + + // Here let's write the code trying to match with Constantin class + // After both classes have been designed around specific needs, see what can be migrated in the parent class and update both children + + FSo100Actuators ActuatorsRestPosition { + 0., + -1.54, + 3.105, + -1.5, + 1.47, + -1.39 + }; + + FSo100Actuators ActuatorsMaxExtendPosition { + 0.0000001, + 0.081027, + -0.01707, + -0.075, + 1.469020, + -1.389073 + }; + + FSo100Actuators ActuatorsDropZone { + PI / 2, + -2.17, + 0.805, + 1.345, + 1.61, + 0 + }; +}; + + diff --git a/Source/LuckyWorldV2/Public/Robot/RobotPawn.h b/Source/LuckyWorldV2/Public/Robot/RobotPawn.h new file mode 100644 index 00000000..80b5177f --- /dev/null +++ b/Source/LuckyWorldV2/Public/Robot/RobotPawn.h @@ -0,0 +1,41 @@ +#pragma once +#include "CoreMinimal.h" +#include "SharedDef.h" +#include "RobotPawn.generated.h" + +class AMujocoVolumeActor; +class URobotPilotComponent; + +// Enum of bots + +UCLASS(Blueprintable) +class LUCKYWORLDV2_API ARobotPawn : public APawn // Should be an actor? +{ + GENERATED_BODY() + +public: + ARobotPawn(); + + virtual void BeginPlay() override; + + // TODO Called by GameInstance after robot has been spawned + void InitRobot(); + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + ERobotsName RobotType = ERobotsName::None; // This value must be set in the pawn + + UPROPERTY(EditAnywhere, BlueprintReadWrite) // TODO Remove UPROPERTY once we migrate physics proxy initialization from Pawn + TObjectPtr PhysicsSceneProxy; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) // TODO Remove UPROPERTY once we migrate physics proxy initialization from Pawn + TObjectPtr RobotActor; // My brain is bleeding facing the fact that we have 2 actors... + + + // ------------------- + // ------ PILOT ------ + // ------------------- + UPROPERTY(EditAnywhere, BlueprintReadWrite) + URobotPilotComponent* RobotPilotComponent = nullptr; + UFUNCTION(BlueprintCallable) + void InitPilotComponent(); // This should have Robot type as parameter? +};