You've already forked LuckyWorld
FT - Looping Episodes
This commit is contained in:
Jb win
@ -12,6 +12,11 @@ URobotPilotSO100Component::URobotPilotSO100Component()
|
||||
void URobotPilotSO100Component::BeginPlay()
|
||||
{
|
||||
Super::BeginPlay();
|
||||
|
||||
// Start the RestPost animation 0.1s after loading
|
||||
AnimationStartTime = .1f;
|
||||
bBreakAfterAnimation = true;
|
||||
RestPose();
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::TickComponent(float DeltaTime, enum ELevelTick TickType,
|
||||
@ -25,23 +30,65 @@ void URobotPilotSO100Component::TickComponent(float DeltaTime, enum ELevelTick T
|
||||
|
||||
FTransform URobotPilotSO100Component::GetReachableTransform()
|
||||
{
|
||||
const auto RobotTransform = RobotOwner->GetActorTransform();
|
||||
const float Yaw = MaxYaw * FMath::RandRange(0., 1.) * (FMath::RandBool() ? 1 : -1);
|
||||
const float Range = MaxRange * FMath::RandRange(0., 1.);
|
||||
const FRotator RandomRotator = FRotator{0,0,Yaw};
|
||||
const FVector RandomLocation = RandomRotator.RotateVector(RobotTransform.GetRotation().GetForwardVector() * Range);
|
||||
const FRotator RandomRotation = UKismetMathLibrary::MakeRotFromXZ(RandomLocation- RobotTransform.GetLocation(), FVector::UpVector);
|
||||
return FTransform(RandomRotation, RandomLocation);
|
||||
const auto RobotTransform = RobotOwner->RobotActor->GetActorTransform();
|
||||
|
||||
// Robot actor is built Y axis forward, like everything in Unreal - Rotate to get Arm facing X world axis aka forward vector
|
||||
const auto ArmWorldRotation = FRotator{0,90,0}.Quaternion() * RobotTransform.GetRotation();
|
||||
|
||||
// Find Arm Pivot Location
|
||||
const auto ArmPivotLocation = RobotTransform.GetLocation() + RobotTransform.GetRotation().RotateVector(PivotOffset) + FVector{0,0,5};
|
||||
|
||||
// Compute a random Yaw
|
||||
const float RandomYaw = MaxYaw * FMath::RandRange(0., 1.) * (FMath::RandBool() ? 1 : -1);
|
||||
const FQuat RandomRotation = FRotator{0,RandomYaw,0}.Quaternion() * ArmWorldRotation;
|
||||
|
||||
|
||||
|
||||
// Compute Random Range within reach of the arm and add this to pivot location
|
||||
// Add a bit more than the Jaw Offset - TODO Offsets must be better computed
|
||||
const float RandomRange = JawOffset.X + MaxRange * FMath::RandRange(0.1f, 1.f);
|
||||
const FVector RandomLocation = ArmPivotLocation + RandomRotation.GetForwardVector() * RandomRange;
|
||||
|
||||
// Find Look at rotation from location to Pivot
|
||||
auto RewardAxis = RandomLocation-ArmPivotLocation;
|
||||
RewardAxis.Z = 0; // Nullify Z to keep a 2D vector -> ensure the geometry roll/pitch are 0
|
||||
const FRotator TowardPivotRotation = UKismetMathLibrary::MakeRotFromXZ(RewardAxis, FVector::UpVector);
|
||||
|
||||
// Debug
|
||||
// DrawDebugLine(this->GetWorld(), ArmPivotLocation + ArmWorldRotation.GetForwardVector() * 70, ArmPivotLocation, FColor::Green, true);
|
||||
// DrawDebugLine(this->GetWorld(), ArmPivotLocation + FVector::UpVector * 70, ArmPivotLocation, FColor::Red, true);
|
||||
// DrawDebugLine(this->GetWorld(), RandomLocation, RandomLocation + TowardPivotRotation.Quaternion().GetForwardVector() * -50 , FColor::Blue, true);
|
||||
|
||||
// Return the Object Transform
|
||||
return FTransform(TowardPivotRotation, RandomLocation);
|
||||
}
|
||||
|
||||
bool URobotPilotSO100Component::GetIsReadyForTraining()
|
||||
{
|
||||
const auto CurrentJoints = GetCurrentJointsFromPhysicsScene();
|
||||
return AreActuatorsAlmostEqual(CurrentJoints, ActuatorsRestPosition);
|
||||
}
|
||||
|
||||
bool URobotPilotSO100Component::GetIsInRestState()
|
||||
{
|
||||
return CurrentAnimationState == 0 && GetIsReadyForTraining();
|
||||
}
|
||||
|
||||
|
||||
void URobotPilotSO100Component::SetTarget(const FTransform& TargetTransformIn)
|
||||
void URobotPilotSO100Component::SetRobotTarget(const FTransform& TargetTransformIn)
|
||||
{
|
||||
// Set Base Values
|
||||
TargetTransform = TargetTransformIn;
|
||||
NextAnimationState();
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::SetRobotCurrentRewardZone(const FTransform& RewardTransformIn)
|
||||
{
|
||||
const auto DirectionToTarget = (RewardTransformIn.GetLocation() - RobotOwner->RobotActor->GetActorLocation()).GetSafeNormal();
|
||||
|
||||
// TODO This is wrong way to do because GetRightVector is the arm forward vector due to robot actor being rotated -90 yaw at spawn
|
||||
bDropZoneIsRight = FVector::DotProduct(RobotOwner->RobotActor->GetActorRotation().Quaternion().GetRightVector(), DirectionToTarget) > 0.f;
|
||||
}
|
||||
|
||||
|
||||
void URobotPilotSO100Component::PrintCurrentActuators() const
|
||||
@ -110,6 +157,18 @@ float URobotPilotSO100Component::GetDeltaSumBetweenActuatorValues(const FSo100Ac
|
||||
return DeltaSum;
|
||||
}
|
||||
|
||||
bool URobotPilotSO100Component::AreActuatorsAlmostEqual(const FSo100Actuators& A, const FSo100Actuators& B)
|
||||
{
|
||||
if (FMath::Abs(A.Rotation) - FMath::Abs(B.Rotation) > 0.001) return false;
|
||||
if (FMath::Abs(A.Pitch) - FMath::Abs(B.Pitch) > 0.001) return false;
|
||||
if (FMath::Abs(A.Elbow) - FMath::Abs(B.Elbow) > 0.001) return false;
|
||||
if (FMath::Abs(A.WristPitch) - FMath::Abs(B.WristPitch) > 0.001) return false;
|
||||
if (FMath::Abs(A.WristRoll) - FMath::Abs(B.WristRoll) > 0.001) return false;
|
||||
if (FMath::Abs(A.Jaw) - FMath::Abs(B.Jaw) > 0.001) return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
double URobotPilotSO100Component::GetControlJointDeltaForActuator(FString ActuatorName) const
|
||||
{
|
||||
auto const Control = RobotOwner->PhysicsSceneProxy->GetActuatorValue(ActuatorName);
|
||||
@ -144,12 +203,25 @@ void URobotPilotSO100Component::PostPhysicStepUpdate(const float SimulationTime)
|
||||
|
||||
if (bHasFinishedAnimation)
|
||||
{
|
||||
if (bBreakAfterAnimation)
|
||||
{
|
||||
bBreakAfterAnimation = false;
|
||||
AnimationStartTime = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// AnimationStartTime = 0.; // Only for debug, can be left here but useless in normal operation mode
|
||||
NextAnimationState();
|
||||
return NextAnimationState();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool URobotPilotSO100Component::IsJawOverCurrent() const
|
||||
{
|
||||
const auto DeltaActJntJaw = GetControlJointDeltaForActuator(Actuator_Jaw);
|
||||
return DeltaActJntJaw > OverCurrentThreshold;
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::NextAnimationState()
|
||||
{
|
||||
const float CurrentPhysicsEngineSceneTime = RobotOwner->PhysicsSceneProxy->GetMujocoData().time;
|
||||
@ -169,15 +241,15 @@ void URobotPilotSO100Component::NextAnimationState()
|
||||
case 4:
|
||||
return OpenJaw();
|
||||
default:
|
||||
return BasePose();
|
||||
return RestPose();
|
||||
}
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::BasePose()
|
||||
void URobotPilotSO100Component::RestPose()
|
||||
{
|
||||
UE_LOG(LogTemp, Log, TEXT("Animate -> BasePose"));
|
||||
CurrentAnimationState = 0;
|
||||
AnimationDuration = 1.5f;
|
||||
AnimationDuration = .5f;
|
||||
AnimTargetRobotActuators = ActuatorsRestPosition;
|
||||
}
|
||||
|
||||
@ -204,7 +276,14 @@ void URobotPilotSO100Component::RotateToTarget()
|
||||
|
||||
// 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);
|
||||
|
||||
// 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 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);
|
||||
|
||||
// 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 !
|
||||
@ -213,7 +292,7 @@ void URobotPilotSO100Component::RotateToTarget()
|
||||
AnimTargetRobotActuators = AnimStartRobotActuators;
|
||||
AnimTargetRobotActuators.Rotation = ActuatorRotation;
|
||||
CurrentAnimationState = 1;
|
||||
AnimationDuration = .7f;
|
||||
AnimationDuration = .33f;
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::MoveToTarget()
|
||||
@ -223,7 +302,8 @@ void URobotPilotSO100Component::MoveToTarget()
|
||||
// Get Pivot World
|
||||
const auto WorldTransform = RobotOwner->RobotActor->GetActorTransform();
|
||||
const FVector PivotWorldLocation = WorldTransform.GetLocation() + WorldTransform.GetRotation().RotateVector(PivotOffset);
|
||||
|
||||
|
||||
// TODO Better computations
|
||||
// Rotate Jaw offset towards target
|
||||
// Get pure 2d rotation
|
||||
RotationToTarget.Pitch = 0;
|
||||
@ -232,7 +312,8 @@ void URobotPilotSO100Component::MoveToTarget()
|
||||
const auto JawPositionWorld = RotationToTarget.RotateVector(JawOffsetToPivot) + PivotWorldLocation;
|
||||
const auto Distance = FVector::Distance(JawPositionWorld, TargetTransform.GetLocation());
|
||||
|
||||
const auto AlphaExtend = FMath::Clamp(Distance / MaxRange, 0., 1.);
|
||||
// TODO Compute correct ranges and avoid to add a hardcoded value to adjust the arm extension
|
||||
const auto AlphaExtend = FMath::Clamp(Distance / (MaxRange + 4), 0., 1.);
|
||||
|
||||
// Set the target actuators values
|
||||
AnimTargetRobotActuators = GetCurrentJointsFromPhysicsScene();
|
||||
@ -244,15 +325,7 @@ void URobotPilotSO100Component::MoveToTarget()
|
||||
|
||||
// Start the animation
|
||||
CurrentAnimationState = 2;
|
||||
AnimationDuration = 2.f;
|
||||
|
||||
DrawDebugLine(
|
||||
this->GetWorld(),
|
||||
JawPositionWorld,
|
||||
JawPositionWorld + (TargetTransform.GetLocation() - JawPositionWorld).GetSafeNormal() * Distance,
|
||||
FColor::Green,
|
||||
true
|
||||
);
|
||||
AnimationDuration = .66f;
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::CloseJaw()
|
||||
@ -268,7 +341,7 @@ void URobotPilotSO100Component::CloseJaw()
|
||||
// Start the animation
|
||||
bDetectOverCurrent = true;
|
||||
CurrentAnimationState = 3;
|
||||
AnimationDuration = 2.f;
|
||||
AnimationDuration = .5f;
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::MoveToDropZone()
|
||||
@ -276,10 +349,13 @@ void URobotPilotSO100Component::MoveToDropZone()
|
||||
UE_LOG(LogTemp, Log, TEXT("Animate -> MoveToDropZone"));
|
||||
|
||||
AnimTargetRobotActuators = ActuatorsDropZone;
|
||||
AnimTargetRobotActuators.Rotation = ActuatorsDropZone.Rotation * (FMath::RandBool() ? 1. : -1.);
|
||||
AnimTargetRobotActuators.Jaw = GetCurrentJointsFromPhysicsScene().Jaw;
|
||||
AnimTargetRobotActuators.Rotation = ActuatorsDropZone.Rotation * (bDropZoneIsRight ? 1. : -1.);
|
||||
|
||||
// Here the Jaw should keep being target to closed
|
||||
AnimTargetRobotActuators.Jaw = ClosedJaw;
|
||||
|
||||
CurrentAnimationState = 4;
|
||||
AnimationDuration = 3.f;
|
||||
AnimationDuration = 1.5f;
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::OpenJaw()
|
||||
@ -288,7 +364,7 @@ void URobotPilotSO100Component::OpenJaw()
|
||||
|
||||
AnimTargetRobotActuators.Jaw = OpenedJaw;
|
||||
CurrentAnimationState = 5;
|
||||
AnimationDuration = 0.6f;
|
||||
AnimationDuration = 0.2f;
|
||||
}
|
||||
|
||||
bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime)
|
||||
@ -296,20 +372,19 @@ 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());
|
||||
const bool bIsArrivedAtActuatorsConfig = AreActuatorsAlmostEqual(AnimTargetRobotActuators, GetCurrentJointsFromPhysicsScene());
|
||||
|
||||
// Alternative Animation completion event - checking for over-current
|
||||
if (bDetectOverCurrent && GetControlJointDeltaForActuator(Actuator_Jaw) > OverCurrentThreshold)
|
||||
if (bDetectOverCurrent && IsJawOverCurrent())
|
||||
{
|
||||
bDetectOverCurrent = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
// UE_LOG(LogTemp, Log, TEXT("Animate -> AnimateActuators %f - %f"), AnimAlpha, GetControlJointDeltaForActuator(Actuator_Jaw));
|
||||
|
||||
// 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;
|
||||
if (AnimAlpha >= 1. && bIsArrivedAtActuatorsConfig) return true;
|
||||
|
||||
// Rotation
|
||||
RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Rotation, FMath::Lerp(
|
||||
@ -347,11 +422,16 @@ bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime)
|
||||
));
|
||||
|
||||
// Jaw
|
||||
RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Jaw, FMath::Lerp(
|
||||
AnimStartRobotActuators.Jaw,
|
||||
AnimTargetRobotActuators.Jaw,
|
||||
AnimAlpha
|
||||
));
|
||||
// If the target Jaw is closed position, aka we are grabbing, but we are over-current, then we don't hold more to not squeeze the object
|
||||
// But if the over-current stops, aka the object rotated a bit, then we hold tighter
|
||||
if (AnimTargetRobotActuators.Jaw != ClosedJaw || !IsJawOverCurrent())
|
||||
{
|
||||
RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Jaw, FMath::Lerp(
|
||||
AnimStartRobotActuators.Jaw,
|
||||
AnimTargetRobotActuators.Jaw,
|
||||
AnimAlpha
|
||||
));
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user