WIP - Architecture for brute force animation
+ sequence look / extend / grab / drop
This commit is contained in:
Jb win
parent
a0478a75b3
commit
0775aeaa7b
Binary file not shown.
@ -14,6 +14,7 @@ void URobotPilotComponent::BeginPlay()
|
||||
|
||||
// Reference owning robot
|
||||
RobotOwner = Cast<ARobotPawn>(GetOwner());
|
||||
InitPilotComponent();
|
||||
}
|
||||
|
||||
void URobotPilotComponent::TickComponent(float DeltaTime, enum ELevelTick TickType,
|
||||
@ -24,18 +25,13 @@ void URobotPilotComponent::TickComponent(float DeltaTime, enum ELevelTick TickTy
|
||||
|
||||
void URobotPilotComponent::InitPilotComponent()
|
||||
{
|
||||
if (RobotOwner.IsValid() && RobotOwner->PhysicSceneProxy.IsValid())
|
||||
if (RobotOwner.Get() && RobotOwner->PhysicSceneProxy.Get())
|
||||
{
|
||||
RobotOwner->PhysicSceneProxy->BindPostPhysicStepDelegate(this, &URobotPilotComponent::AnimateActuators);
|
||||
RobotOwner->PhysicSceneProxy->BindPostPhysicStepDelegate(this, &URobotPilotComponent::PostPhysicStepUpdate);
|
||||
}
|
||||
}
|
||||
|
||||
void URobotPilotComponent::StartAnimation(const FRobotActuators& NewAnimationTarget)
|
||||
void URobotPilotComponent::PostPhysicStepUpdate(const float SimulationTime)
|
||||
{
|
||||
AnimTargetRobotActuators = NewAnimationTarget;
|
||||
}
|
||||
|
||||
void URobotPilotComponent::AnimateActuators(float SimulationTime)
|
||||
{
|
||||
// Override in each dedicated RobotPilotComponent
|
||||
// Overriden in each dedicated RobotPilotComponent
|
||||
}
|
||||
|
||||
@ -1,5 +1,10 @@
|
||||
#include "Robot/PilotComponent/RobotPilotSO100Component.h"
|
||||
|
||||
#include "Actors/MujocoVolumeActor.h"
|
||||
#include "Kismet/KismetMathLibrary.h"
|
||||
#include "Kismet/KismetSystemLibrary.h"
|
||||
#include "Robot/RobotPawn.h"
|
||||
|
||||
URobotPilotSO100Component::URobotPilotSO100Component()
|
||||
{
|
||||
|
||||
@ -16,7 +21,177 @@ void URobotPilotSO100Component::TickComponent(float DeltaTime, enum ELevelTick T
|
||||
Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::StartAnimation(const FRobotActuators& NewAnimationTarget)
|
||||
void URobotPilotSO100Component::SetTarget(const FTransform& TargetTransformIn)
|
||||
{
|
||||
// Super::StartAnimation(NewAnimationTarget);
|
||||
// Set Base Values
|
||||
AnimStartRobotActuators = GetCurrentActuatorsFromPhysicScene();
|
||||
TargetTransform = TargetTransformIn;
|
||||
NextAnimationState();
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::PrintActuators() const
|
||||
{
|
||||
const auto [Rotation, Pitch, Elbow, WristPitch, WristRoll, Jaw] = GetCurrentActuatorsFromPhysicScene();
|
||||
UE_LOG(LogTemp, Display, TEXT("Actuators Rotation %f | Pitch %f | Elbow %f | WristPitch %f | WristRoll %f | Jaw %f"),
|
||||
Rotation,
|
||||
Pitch,
|
||||
Elbow,
|
||||
WristPitch,
|
||||
WristRoll,
|
||||
Jaw
|
||||
);
|
||||
}
|
||||
|
||||
FSo100Actuators URobotPilotSO100Component::GetCurrentActuatorsFromPhysicScene() 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),
|
||||
};
|
||||
}
|
||||
|
||||
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 && CurrentAnimationState < MaxAnimationState)
|
||||
{
|
||||
NextAnimationState();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::NextAnimationState()
|
||||
{
|
||||
const float CurrentSimulationTime = RobotOwner->PhysicSceneProxy->GetMujocoData().time;
|
||||
AnimationStartTime = CurrentSimulationTime;
|
||||
|
||||
switch (CurrentAnimationState)
|
||||
{
|
||||
case 0:
|
||||
return RotateToTarget();
|
||||
case 1:
|
||||
return CloseJaw();
|
||||
case 2:
|
||||
return MoveToDropZone();
|
||||
case 3:
|
||||
return OpenJaw();
|
||||
case 4:
|
||||
return OpenJaw();
|
||||
default:
|
||||
return BasePose();
|
||||
}
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::BasePose()
|
||||
{
|
||||
CurrentAnimationState = 0;
|
||||
AnimationDuration = 1.5f;
|
||||
AnimTargetRobotActuators = ActuatorsRestPosition;
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
|
||||
// FVector::CosineAngle2D()
|
||||
const auto Angle = UKismetMathLibrary::FindLookAtRotation(
|
||||
WorldTransform.GetRotation().GetForwardVector(),
|
||||
TargetTransform.GetLocation() - PivotWorldLocation
|
||||
);
|
||||
|
||||
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);
|
||||
|
||||
DrawDebugLine(
|
||||
this->GetWorld(),
|
||||
PivotWorldLocation + 100 * FVector::UpVector,
|
||||
PivotWorldLocation,
|
||||
FColor::Blue,
|
||||
true);
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::CloseJaw()
|
||||
{
|
||||
// Here we need overcurrent detection - not here actually
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::MoveToDropZone()
|
||||
{
|
||||
const auto DropZoneActuatorsSettings = FMath::RandBool() ? ActuatorsLeftDropZone : ActuatorsRightDropZone;
|
||||
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::OpenJaw()
|
||||
{
|
||||
}
|
||||
|
||||
bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime) const
|
||||
{
|
||||
const double AnimAlpha = FMath::Clamp((SimulationTime - AnimationStartTime) / AnimationDuration, 0., 1.);
|
||||
|
||||
// Stop the animation if we reached the target
|
||||
if (AnimAlpha >= 1.) return true;
|
||||
|
||||
// Rotation
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Rotation, FMath::Lerp(
|
||||
AnimStartRobotActuators.Rotation,
|
||||
AnimTargetRobotActuators.Rotation,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
// Pitch
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Pitch, FMath::Lerp(
|
||||
AnimStartRobotActuators.Pitch,
|
||||
AnimTargetRobotActuators.Pitch,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
// Elbow
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Elbow, FMath::Lerp(
|
||||
AnimStartRobotActuators.Elbow,
|
||||
AnimTargetRobotActuators.Elbow,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
// WristPitch
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_WristPitch, FMath::Lerp(
|
||||
AnimStartRobotActuators.WristPitch,
|
||||
AnimTargetRobotActuators.WristPitch,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
// WristRoll
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_WristRoll, FMath::Lerp(
|
||||
AnimStartRobotActuators.WristRoll,
|
||||
AnimTargetRobotActuators.WristRoll,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
// Jaw
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Jaw, FMath::Lerp(
|
||||
AnimStartRobotActuators.Jaw,
|
||||
AnimTargetRobotActuators.Jaw,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -28,11 +28,11 @@ void ARobotPawn::InitPilotComponent()
|
||||
break;
|
||||
|
||||
case ERobotsName::SO100Robot:
|
||||
RobotPilotComponent = NewObject<URobotPilotSO100Component>(GetOwner());
|
||||
RobotPilotComponent = NewObject<URobotPilotSO100Component>(this);
|
||||
break;
|
||||
|
||||
case ERobotsName::DJIDrone:
|
||||
RobotPilotComponent = NewObject<URobotPilotMultiRotorDrone>(GetOwner());
|
||||
RobotPilotComponent = NewObject<URobotPilotMultiRotorDrone>(this);
|
||||
break;
|
||||
|
||||
case ERobotsName::Luck_e:
|
||||
@ -63,6 +63,5 @@ void ARobotPawn::InitPilotComponent()
|
||||
if (RobotPilotComponent)
|
||||
{
|
||||
RobotPilotComponent->RegisterComponent();
|
||||
RobotPilotComponent->InitPilotComponent();
|
||||
}
|
||||
}
|
||||
|
||||
@ -25,22 +25,16 @@ public:
|
||||
virtual void BeginPlay() override;
|
||||
virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;
|
||||
virtual void InitPilotComponent();
|
||||
virtual void PostPhysicStepUpdate(const float SimulationTime);
|
||||
|
||||
private:
|
||||
protected: // Child class need access
|
||||
// Only to easy access within the component
|
||||
TWeakObjectPtr<ARobotPawn> RobotOwner = nullptr;
|
||||
TObjectPtr<ARobotPawn> RobotOwner = nullptr;
|
||||
|
||||
// ----------------
|
||||
// ----- ANIM -----
|
||||
// ----------------
|
||||
public:
|
||||
virtual void StartAnimation(const FRobotActuators& NewAnimationTarget);
|
||||
|
||||
private:
|
||||
virtual void AnimateActuators(float SimulationTime); // Bound to the PhysicProxy post-update delegate
|
||||
float AnimationDuration = 0.f;
|
||||
protected: // Child class need access
|
||||
float AnimationStartTime = 0.f;
|
||||
FRobotActuators CurrentRobotActuators; // This will be updated by the post-physic delegate
|
||||
FRobotActuators AnimStartRobotActuators;
|
||||
FRobotActuators AnimTargetRobotActuators;
|
||||
float AnimationDuration = 0.f;
|
||||
};
|
||||
|
||||
@ -4,26 +4,26 @@
|
||||
#include "RobotPilotSO100Component.generated.h"
|
||||
|
||||
USTRUCT(BlueprintType)
|
||||
struct FSo100Actuators : public FRobotActuators
|
||||
struct FSo100Actuators
|
||||
{
|
||||
GENERATED_BODY()
|
||||
UPROPERTY(EditAnywhere, BlueprintReadWrite)
|
||||
float Rotation = 0.f;
|
||||
double Rotation = 0.;
|
||||
|
||||
UPROPERTY(EditAnywhere, BlueprintReadWrite)
|
||||
float Pitch = 0.f;
|
||||
double Pitch = 0.;
|
||||
|
||||
UPROPERTY(EditAnywhere, BlueprintReadWrite)
|
||||
float Elbow = 0.f;
|
||||
double Elbow = 0.;
|
||||
|
||||
UPROPERTY(EditAnywhere, BlueprintReadWrite)
|
||||
float WristPitch = 0.f;
|
||||
double WristPitch = 0.;
|
||||
|
||||
UPROPERTY(EditAnywhere, BlueprintReadWrite)
|
||||
float WristRoll = 0.f;
|
||||
double WristRoll = 0.;
|
||||
|
||||
UPROPERTY(EditAnywhere, BlueprintReadWrite)
|
||||
float Jaw = 0.f;
|
||||
double Jaw = 0.;
|
||||
};
|
||||
|
||||
UCLASS(Blueprintable)
|
||||
@ -38,13 +38,113 @@ class LUCKYWORLDV2_API URobotPilotSO100Component : public URobotPilotComponent
|
||||
virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;
|
||||
|
||||
UFUNCTION(BlueprintCallable)
|
||||
virtual void StartAnimation(const FRobotActuators& NewAnimationTarget) override;
|
||||
void SetTarget(const FTransform& TargetTransformIn);
|
||||
private:
|
||||
FTransform TargetTransform;
|
||||
|
||||
public:
|
||||
UFUNCTION(BlueprintCallable)
|
||||
void PrintActuators() const;
|
||||
|
||||
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");
|
||||
|
||||
// SO100 Static Variables
|
||||
FVector PivotOffset = FVector{-0.000030, 4.520021, 1.650041};
|
||||
|
||||
/**
|
||||
* Query the physic proxy on the RobotOwner to get the SO100 actuators values
|
||||
* @return
|
||||
*/
|
||||
FSo100Actuators GetCurrentActuatorsFromPhysicScene() const;
|
||||
|
||||
// Called after every physic step
|
||||
virtual void PostPhysicStepUpdate(const float SimulationTime) override;
|
||||
bool AnimateActuators(float SimulationTime) const; // Bound to the PhysicProxy post-update delegate
|
||||
FSo100Actuators CurrentRobotActuators; // This will be updated by the post-physic delegate
|
||||
FSo100Actuators AnimStartRobotActuators;
|
||||
FSo100Actuators AnimTargetRobotActuators;
|
||||
|
||||
// Quick and dirty sequence of moves
|
||||
// -1 -> Start Game, extended
|
||||
// 0 -> Retract - base pose
|
||||
// 1 -> Rotate towards goal
|
||||
// 2 -> Move to target
|
||||
// 3 -> Close the jaw
|
||||
// 4 -> Go to drop zone
|
||||
// 5 -> open jaw
|
||||
int32 CurrentAnimationState = -1;
|
||||
int32 MaxAnimationState = 5;
|
||||
void NextAnimationState();
|
||||
|
||||
void BasePose();
|
||||
void RotateToTarget();
|
||||
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.,
|
||||
0.,
|
||||
0.,
|
||||
0.,
|
||||
-1.56,
|
||||
-1.045
|
||||
};
|
||||
|
||||
FSo100Actuators ActuatorsLeftDropZone {
|
||||
0.,
|
||||
0.,
|
||||
0.,
|
||||
0.,
|
||||
-1.56,
|
||||
-1.045
|
||||
};
|
||||
|
||||
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
|
||||
// Move the arm myself JB + Capture LOG
|
||||
|
||||
// Set the target in world absolute <- get actor of class with tag
|
||||
// Compute target relative
|
||||
// Compute distance to target
|
||||
// compute rotation - direction to target
|
||||
// lerp values to go grab
|
||||
// close jaw
|
||||
// select drop zone
|
||||
// move to drop zone (pick bool)
|
||||
// open jaw
|
||||
// assess success or failure
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -25,7 +25,11 @@ public:
|
||||
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
|
||||
TWeakObjectPtr<AMujocoVolumeActor> PhysicSceneProxy;
|
||||
TObjectPtr<AMujocoVolumeActor> PhysicSceneProxy;
|
||||
|
||||
UPROPERTY(EditAnywhere, BlueprintReadWrite) // TODO Remove UPROPERTY once we migrate physics proxy initialization from Pawn
|
||||
TObjectPtr<AActor> RobotActor; // My brain is bleeding facing the fact that we have 2 actors...
|
||||
|
||||
|
||||
// -------------------
|
||||
// ------ PILOT ------
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user