LuckyRobots/LuckyWorld
11
2
Fork 0
Code Issues Pull Requests Actions Packages Projects 1 Releases Wiki Activity

FT - Looping Episodes

Browse Source
This commit is contained in:
Jb win
2025-05-04 01:04:44 +07:00
parent 45490051cd
commit 9806b4f5af
17 changed files with 454 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h
View File

@ -37,13 +37,16 @@ public:
virtual void BeginPlay() override;
virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;
virtual FTransform GetReachableTransform() override;
virtual bool GetIsReadyForTraining() override;
virtual bool GetIsInRestState() override;
virtual void SetRobotTarget(const FTransform& TargetTransformIn) override;
virtual void SetRobotCurrentRewardZone(const FTransform& RewardTransformIn) override;
UFUNCTION(BlueprintCallable)
void SetTarget(const FTransform& TargetTransformIn);
private:
FTransform TargetTransform;
bool bDropZoneIsRight = false;
//---------------------
//------- DEBUG -------
@ -66,14 +69,17 @@ private:
FString Actuator_Jaw = FString("Jaw");
// SO100 Static Variables
FVector PivotOffset = FVector{-0.000030, 4.520021, 1.650041};
FVector JawOffset = FVector{23, 2, 9};
// TODO Those values must be more precise, and probably that the way we compute the rotation is not good enough
// TODO Let's discuss how to improve that algorithm
FVector PivotOffset = FVector{-0.000030, 4.520021, 1.650041}; // From the Robot Location
FVector JawOffset = FVector{23, 2, 9}; // From the Pivot
float MaxRange = 20.757929; // fixed_jaw_pad_3 ForwardVectorLength Delta between Rest and MaxExtend
float MaxYaw = 150.f;
float MaxYaw = 80.f;
// Actuators Joints and Controls are expressed in doubles
double ClosedJaw = 0.18;
double ClosedJaw = -0.01;
double OpenedJaw = -2.0;
int32 JawState = 0; // 0 - Opened || 1 - Grabbing
/**
* Query the physic proxy on the RobotOwner to get the SO100 actuators values
@ -85,9 +91,10 @@ private:
FSo100Actuators GetCurrentJointsFromPhysicsScene() const;
double GetControlJointDeltaForActuator(FString ActuatorName) const;
static float GetDeltaSumBetweenActuatorValues(const FSo100Actuators& A, const FSo100Actuators& B);
static bool AreActuatorsAlmostEqual(const FSo100Actuators& A, const FSo100Actuators& B);
static FSo100Actuators LerpActuators(const FSo100Actuators& A, const FSo100Actuators& B, const float Alpha);
// Called after every physic step
// Called after every physic step - this is a substep tick
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
@ -99,7 +106,8 @@ private:
// ----- OVER-CURRENT -----
// ------------------------
bool bDetectOverCurrent = false;
const float OverCurrentThreshold = 0.15;
const float OverCurrentThreshold = 0.1;
bool IsJawOverCurrent() const;
// Quick and dirty sequence of moves
// -1 -> Start Game, extended
@ -111,8 +119,9 @@ private:
// 5 -> open jaw
int32 CurrentAnimationState = -1;
void NextAnimationState();
void BasePose();
bool bBreakAfterAnimation = false;
void RestPose();
void RotateToTarget();
void MoveToTarget();
void CloseJaw();
@ -143,7 +152,7 @@ private:
};
FSo100Actuators ActuatorsDropZone {
PI / 2,
PI / 2 + 0.25,
-2.17,
0.805,
1.345,