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

FIX - Robot can detect over-current again

Browse Source
This commit is contained in:
JB Briant
2025-05-10 16:29:03 +07:00
parent cf9a38df60
commit ac7b49d924
7 changed files with 38 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
Content/Blueprint/Robots/So100/BP-Mujoco-Robot-so100.uasset
View File

Binary file not shown.

BIN
Content/Blueprint/Robots/So100/BP-RobotPawn-so100.uasset
View File

Binary file not shown.

34
Source/LuckyWorldV2/Private/Robot/So100/RobotPilotCmpSo100.ActuatorUtils.cpp
View File

@ -5,13 +5,14 @@
void URobotPilotCmpSo100::PrintCurrentActuators() const
{
PrintActuators(GetCurrentControlsFromPhysicScene());
PrintActuators(GetCurrentControlsFromPhysicScene(), FString(""));
}
void URobotPilotCmpSo100::PrintActuators(FSo100Actuators Actuators)
void URobotPilotCmpSo100::PrintActuators(FSo100Actuators Actuators, const FString& Prefix = FString(""))
{
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"),
UE_LOG(LogTemp, Display, TEXT("[%s] Actuators Rotation %f | Pitch %f | Elbow %f | WristPitch %f | WristRoll %f | Jaw %f"),
*Prefix,
Rotation,
Pitch,
Elbow,
@ -63,14 +64,18 @@ float URobotPilotCmpSo100::GetDeltaSumBetweenActuatorValues(const FSo100Actuator
return DeltaSum;
}
bool URobotPilotCmpSo100::AreActuatorsAlmostEqual(const FSo100Actuators& A, const FSo100Actuators& B)
bool URobotPilotCmpSo100::AreActuatorsAlmostEqual(const FSo100Actuators& A, const FSo100Actuators& B, const bool bIgnoreJaw = false)
{
if (FMath::Abs(A.Rotation) - FMath::Abs(B.Rotation) > 0.01) return false;
if (FMath::Abs(A.Pitch) - FMath::Abs(B.Pitch) > 0.01) return false;
if (FMath::Abs(A.Elbow) - FMath::Abs(B.Elbow) > 0.01) return false;
if (FMath::Abs(A.WristPitch) - FMath::Abs(B.WristPitch) > 0.01) return false;
if (FMath::Abs(A.WristRoll) - FMath::Abs(B.WristRoll) > 0.01) return false;
if (FMath::Abs(A.Jaw) - FMath::Abs(B.Jaw) > 0.01) return false;
if (FMath::Abs(A.Rotation) - FMath::Abs(B.Rotation) > 0.03) return false;
if (FMath::Abs(A.Pitch) - FMath::Abs(B.Pitch) > 0.03) return false;
if (FMath::Abs(A.Elbow) - FMath::Abs(B.Elbow) > 0.03) return false;
if (FMath::Abs(A.WristPitch) - FMath::Abs(B.WristPitch) > 0.03) return false;
if (FMath::Abs(A.WristRoll) - FMath::Abs(B.WristRoll) > 0.03) return false;
if (!bIgnoreJaw)
{
if (FMath::Abs(A.Jaw) - FMath::Abs(B.Jaw) > 0.03) return false;
}
return true;
}
@ -79,7 +84,7 @@ double URobotPilotCmpSo100::GetControlJointDeltaForActuator(const FString& Actua
{
auto const Control = RobotOwner->PhysicsSceneProxy->GetActuatorValue(ActuatorName);
auto const Joint = RobotOwner->PhysicsSceneProxy->GetJointValue(ActuatorName);
return Control -Joint;
return Joint - Control;
}
FSo100Actuators URobotPilotCmpSo100::LerpActuators(
@ -98,8 +103,9 @@ FSo100Actuators URobotPilotCmpSo100::LerpActuators(
};
}
bool URobotPilotCmpSo100::IsJawOverCurrent() const
bool URobotPilotCmpSo100::IsJawOverCurrent()
{
const auto DeltaActJntJaw = GetControlJointDeltaForActuator(Actuator_Jaw);
return DeltaActJntJaw > OverCurrentThreshold;
auto const Control = RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Jaw);
auto const Joint = RobotOwner->PhysicsSceneProxy->GetJointValue(Actuator_Jaw);
return Joint - Control > OverCurrentThreshold;
}

12
Source/LuckyWorldV2/Private/Robot/So100/RobotPilotCmpSo100.Animation.cpp
View File

@ -62,11 +62,11 @@ void URobotPilotCmpSo100::RotateToTarget()
// TODO Better computation of Jaw Center would avoid that ugliness
// TODO This is not working for values Dot<0.15 - we need a better solution
const auto ModBaseAlpha = FMath::Abs(Dot) / 0.3;
const auto ModBaseAlpha = FMath::Abs(Dot) / 0.66;
const auto ModBase = 0.1 * FMath::Lerp(5.f, 1.f, FMath::Min(ModBaseAlpha, 1)); // TODO Dot Product below 0.5 needs more yaw modification
const auto Mod = ModBase * (Dot <= 0 ? 1 : -1); // TODO Hardcoded value - compute better Jaw Offset and robot geometry awareness
UE_LOG(LogTemp, Log, TEXT("Dot : %f | ModBaseAlpha: %f | ModBase: %f | Mod: %f"), Dot, ModBaseAlpha, ModBase, Mod);
// UE_LOG(LogTemp, Log, TEXT("Dot : %f | ModBaseAlpha: %f | ModBase: %f | Mod: %f"), Dot, ModBaseAlpha, ModBase, Mod);
// 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 !
@ -104,7 +104,7 @@ void URobotPilotCmpSo100::MoveToTarget()
AnimTargetRobotActuators.Rotation = AnimStartRobotActuators.Rotation;
PrintActuators(AnimTargetRobotActuators);
PrintActuators(AnimTargetRobotActuators, "MoveToTarget");
// Start the animation
CurrentAnimationState = 2;
@ -124,7 +124,7 @@ void URobotPilotCmpSo100::CloseJaw()
// Start the animation
bDetectOverCurrent = true;
CurrentAnimationState = 3;
AnimationDuration = .5f;
AnimationDuration = 1.f;
}
void URobotPilotCmpSo100::MoveToDropZone()
@ -138,7 +138,7 @@ void URobotPilotCmpSo100::MoveToDropZone()
AnimTargetRobotActuators.Jaw = ClosedJaw;
CurrentAnimationState = 4;
AnimationDuration = 1.5f;
AnimationDuration = 2.5f;
}
void URobotPilotCmpSo100::OpenJaw()
@ -147,5 +147,5 @@ void URobotPilotCmpSo100::OpenJaw()
AnimTargetRobotActuators.Jaw = OpenedJaw;
CurrentAnimationState = 5;
AnimationDuration = 0.2f;
AnimationDuration = .3f;
}

2
Source/LuckyWorldV2/Private/Robot/So100/RobotPilotCmpSo100.Training.cpp
View File

@ -35,7 +35,7 @@ FTransform URobotPilotCmpSo100::GetReachableTransform()
bool URobotPilotCmpSo100::GetIsReadyForTraining()
{
const auto CurrentJoints = GetCurrentJointsFromPhysicsScene();
return AreActuatorsAlmostEqual(CurrentJoints, ActuatorsRestPosition);
return AreActuatorsAlmostEqual(CurrentJoints, ActuatorsRestPosition, false);
}
bool URobotPilotCmpSo100::GetIsInRestState()

7
Source/LuckyWorldV2/Private/Robot/So100/RobotPilotCmpSo100.cpp
View File

@ -71,13 +71,8 @@ bool URobotPilotCmpSo100::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 bool bIsArrivedAtActuatorsConfig = AreActuatorsAlmostEqual(AnimTargetRobotActuators, GetCurrentJointsFromPhysicsScene());
const bool bIsArrivedAtActuatorsConfig = AreActuatorsAlmostEqual(AnimTargetRobotActuators, GetCurrentJointsFromPhysicsScene(), true);
// float DeltaSum = GetDeltaSumBetweenActuatorValues(AnimTargetRobotActuators, GetCurrentJointsFromPhysicsScene());
PrintActuators(AnimTargetRobotActuators);
PrintActuators(GetCurrentJointsFromPhysicsScene());
// UE_LOG(LogTemp, Log, TEXT("AnimAlpha %f bIsArrivedAtActuatorsConfig %i"), AnimAlpha, bIsArrivedAtActuatorsConfig);
// Alternative Animation completion event - checking for over-current
if (bDetectOverCurrent && IsJawOverCurrent())
{

19
Source/LuckyWorldV2/Public/Robot/So100/RobotPilotCmpSo100.h
View File

@ -76,7 +76,7 @@ private:
public:
UFUNCTION(BlueprintCallable)
void PrintCurrentActuators() const;
static void PrintActuators(FSo100Actuators Actuators);
static void PrintActuators(FSo100Actuators Actuators, const FString& Prefix);
UFUNCTION(BlueprintCallable)
void DisableAnim();
@ -99,9 +99,10 @@ private:
float MaxYaw = 80.f;
// Actuators Joints and Controls are expressed in doubles
double ClosedJaw = 0.01;
double OpenedJaw = 1.5;
double ClosedJaw = -0.05;
double OpenedJaw = 1.6;
int32 JawState = 0; // 0 - Opened || 1 - Grabbing
double LastFrameJawJoint = 0.0;
/**
* Query the physic proxy on the RobotOwner to get the SO100 actuators values
@ -113,7 +114,7 @@ private:
FSo100Actuators GetCurrentJointsFromPhysicsScene() const;
double GetControlJointDeltaForActuator(const FString& ActuatorName) const;
static float GetDeltaSumBetweenActuatorValues(const FSo100Actuators& A, const FSo100Actuators& B);
static bool AreActuatorsAlmostEqual(const FSo100Actuators& A, const FSo100Actuators& B);
static bool AreActuatorsAlmostEqual(const FSo100Actuators& A, const FSo100Actuators& B, bool bIgnoreJaw);
static FSo100Actuators LerpActuators(const FSo100Actuators& A, const FSo100Actuators& B, const float Alpha);
// Called after every physic step - this is a substep tick
@ -128,8 +129,8 @@ private:
// ----- OVER-CURRENT -----
// ------------------------
bool bDetectOverCurrent = false;
const float OverCurrentThreshold = 0.1;
bool IsJawOverCurrent() const;
const float OverCurrentThreshold = 0.2;
bool IsJawOverCurrent();
// Quick and dirty sequence of moves
// -1 -> Start Game, extended
@ -180,9 +181,9 @@ private:
PI / 2 + 0.25,
-2.17,
1.1,
0.66,
1.146,
0
1.04,
1.676,
OpenedJaw
};
};