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

MISC - Rename Variables

Browse Source
This commit is contained in:
Jb win 2025-05-02 00:21:59 +07:00
parent d97a4ca0f8
commit 90cff49bc9
5 changed files with 50 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset
View File

Binary file not shown.

4
Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp
View File

@ -25,9 +25,9 @@ void URobotPilotComponent::TickComponent(float DeltaTime, enum ELevelTick TickTy
void URobotPilotComponent::InitPilotComponent() void URobotPilotComponent::InitPilotComponent()
{ {
if (RobotOwner.Get() && RobotOwner->PhysicSceneProxy.Get()) if (RobotOwner.Get() && RobotOwner->PhysicsSceneProxy.Get())
{ {
RobotOwner->PhysicSceneProxy->BindPostPhysicStepDelegate(this, &URobotPilotComponent::PostPhysicStepUpdate); RobotOwner->PhysicsSceneProxy->BindPostPhysicStepDelegate(this, &URobotPilotComponent::PostPhysicStepUpdate);
} }
} }

71
Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp
View File

@ -21,7 +21,7 @@ void URobotPilotSO100Component::TickComponent(float DeltaTime, enum ELevelTick T
FActorComponentTickFunction* ThisTickFunction) FActorComponentTickFunction* ThisTickFunction)
{ {
// Super::TickComponent(DeltaTime, TickType, ThisTickFunction); // Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
// const auto Joints = GetCurrentJointsFromPhysicScene(); // const auto Joints = GetCurrentJointsFromPhysicsScene();
// const auto Controls = GetCurrentControlsFromPhysicScene(); // const auto Controls = GetCurrentControlsFromPhysicScene();
// UE_LOG(LogTemp, Log, TEXT("Joint: %f - Control: %f - Delta: %f"), Joints.Jaw, Controls.Jaw, Controls.Jaw - Joints.Jaw); // UE_LOG(LogTemp, Log, TEXT("Joint: %f - Control: %f - Delta: %f"), Joints.Jaw, Controls.Jaw, Controls.Jaw - Joints.Jaw);
} }
@ -59,31 +59,46 @@ void URobotPilotSO100Component::DisableAnim()
AnimationStartTime = 0; 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 FSo100Actuators URobotPilotSO100Component::GetCurrentControlsFromPhysicScene() const
{ {
return FSo100Actuators return FSo100Actuators
{ {
RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Rotation), RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Rotation),
RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Pitch), RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Pitch),
RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Elbow), RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Elbow),
RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_WristPitch), RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_WristPitch),
RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_WristRoll), RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_WristRoll),
RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Jaw), RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Jaw),
}; };
} }
FSo100Actuators URobotPilotSO100Component::GetCurrentJointsFromPhysicScene() const 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 // 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" // Joints get a "1" appended to their name -> refacto so it's "_Joint"
return FSo100Actuators return FSo100Actuators
{ {
RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Rotation).Append("1")), RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Rotation).Append("1")),
RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Pitch).Append("1")), RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Pitch).Append("1")),
RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Elbow).Append("1")), RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Elbow).Append("1")),
RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_WristPitch).Append("1")), RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_WristPitch).Append("1")),
RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_WristRoll).Append("1")), RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_WristRoll).Append("1")),
RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Jaw).Append("1")) RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Jaw).Append("1"))
}; };
} }
@ -103,8 +118,8 @@ float URobotPilotSO100Component::GetDeltaSumBetweenActuatorValues(const FSo100Ac
double URobotPilotSO100Component::GetControlJointDeltaForActuator(FString ActuatorName) const double URobotPilotSO100Component::GetControlJointDeltaForActuator(FString ActuatorName) const
{ {
auto const Control = RobotOwner->PhysicSceneProxy->GetActuatorValue(ActuatorName); auto const Control = RobotOwner->PhysicsSceneProxy->GetActuatorValue(ActuatorName);
auto const Joint = RobotOwner->PhysicSceneProxy->GetJointValue(ActuatorName.Append("1")); auto const Joint = RobotOwner->PhysicsSceneProxy->GetJointValue(ActuatorName.Append("1"));
return Control -Joint; return Control -Joint;
} }
@ -143,9 +158,9 @@ void URobotPilotSO100Component::PostPhysicStepUpdate(const float SimulationTime)
void URobotPilotSO100Component::NextAnimationState() void URobotPilotSO100Component::NextAnimationState()
{ {
const float CurrentSimulationTime = RobotOwner->PhysicSceneProxy->GetMujocoData().time; const float CurrentPhysicsEngineSceneTime = RobotOwner->PhysicsSceneProxy->GetMujocoData().time;
AnimationStartTime = CurrentSimulationTime; AnimationStartTime = CurrentPhysicsEngineSceneTime;
AnimStartRobotActuators = GetCurrentJointsFromPhysicScene(); AnimStartRobotActuators = GetCurrentJointsFromPhysicsScene();
switch (CurrentAnimationState) switch (CurrentAnimationState)
{ {
@ -226,7 +241,7 @@ void URobotPilotSO100Component::MoveToTarget()
const auto AlphaExtend = FMath::Clamp(Distance / MaxReach, 0., 1.); const auto AlphaExtend = FMath::Clamp(Distance / MaxReach, 0., 1.);
// Set the target actuators values // Set the target actuators values
AnimTargetRobotActuators = GetCurrentJointsFromPhysicScene(); AnimTargetRobotActuators = GetCurrentJointsFromPhysicsScene();
AnimTargetRobotActuators = LerpActuators(ActuatorsRestPosition, ActuatorsMaxExtendPosition, AlphaExtend); AnimTargetRobotActuators = LerpActuators(ActuatorsRestPosition, ActuatorsMaxExtendPosition, AlphaExtend);
@ -268,7 +283,7 @@ void URobotPilotSO100Component::MoveToDropZone()
AnimTargetRobotActuators = ActuatorsDropZone; AnimTargetRobotActuators = ActuatorsDropZone;
AnimTargetRobotActuators.Rotation = ActuatorsDropZone.Rotation * (FMath::RandBool() ? 1. : -1.); AnimTargetRobotActuators.Rotation = ActuatorsDropZone.Rotation * (FMath::RandBool() ? 1. : -1.);
AnimTargetRobotActuators.Jaw = GetCurrentJointsFromPhysicScene().Jaw; AnimTargetRobotActuators.Jaw = GetCurrentJointsFromPhysicsScene().Jaw;
CurrentAnimationState = 4; CurrentAnimationState = 4;
AnimationDuration = 3.f; AnimationDuration = 3.f;
} }
@ -287,7 +302,7 @@ bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime)
const double AnimAlpha = FMath::Clamp((SimulationTime - AnimationStartTime) / AnimationDuration, 0., 1.); 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 // Need to wait for the joints to be in the right position before switching to next animation
const auto DeltaSum = GetDeltaSumBetweenActuatorValues(AnimTargetRobotActuators, GetCurrentJointsFromPhysicScene()); const auto DeltaSum = GetDeltaSumBetweenActuatorValues(AnimTargetRobotActuators, GetCurrentJointsFromPhysicsScene());
// Alternative Animation completion event - checking for over-current // Alternative Animation completion event - checking for over-current
if (bDetectOverCurrent && GetControlJointDeltaForActuator(Actuator_Jaw) > OverCurrentThreshold) if (bDetectOverCurrent && GetControlJointDeltaForActuator(Actuator_Jaw) > OverCurrentThreshold)
@ -303,42 +318,42 @@ bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime)
if (AnimAlpha >= 1. && DeltaSum <= .001) return true; if (AnimAlpha >= 1. && DeltaSum <= .001) return true;
// Rotation // Rotation
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Rotation, FMath::Lerp( RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Rotation, FMath::Lerp(
AnimStartRobotActuators.Rotation, AnimStartRobotActuators.Rotation,
AnimTargetRobotActuators.Rotation, AnimTargetRobotActuators.Rotation,
AnimAlpha AnimAlpha
)); ));
// Pitch // Pitch
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Pitch, FMath::Lerp( RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Pitch, FMath::Lerp(
AnimStartRobotActuators.Pitch, AnimStartRobotActuators.Pitch,
AnimTargetRobotActuators.Pitch, AnimTargetRobotActuators.Pitch,
AnimAlpha AnimAlpha
)); ));
// Elbow // Elbow
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Elbow, FMath::Lerp( RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Elbow, FMath::Lerp(
AnimStartRobotActuators.Elbow, AnimStartRobotActuators.Elbow,
AnimTargetRobotActuators.Elbow, AnimTargetRobotActuators.Elbow,
AnimAlpha AnimAlpha
)); ));
// WristPitch // WristPitch
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_WristPitch, FMath::Lerp( RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_WristPitch, FMath::Lerp(
AnimStartRobotActuators.WristPitch, AnimStartRobotActuators.WristPitch,
AnimTargetRobotActuators.WristPitch, AnimTargetRobotActuators.WristPitch,
AnimAlpha AnimAlpha
)); ));
// WristRoll // WristRoll
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_WristRoll, FMath::Lerp( RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_WristRoll, FMath::Lerp(
AnimStartRobotActuators.WristRoll, AnimStartRobotActuators.WristRoll,
AnimTargetRobotActuators.WristRoll, AnimTargetRobotActuators.WristRoll,
AnimAlpha AnimAlpha
)); ));
// Jaw // Jaw
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Jaw, FMath::Lerp( RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Jaw, FMath::Lerp(
AnimStartRobotActuators.Jaw, AnimStartRobotActuators.Jaw,
AnimTargetRobotActuators.Jaw, AnimTargetRobotActuators.Jaw,
AnimAlpha AnimAlpha

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

@ -52,6 +52,9 @@ public:
UFUNCTION(BlueprintCallable) UFUNCTION(BlueprintCallable)
void DisableAnim(); void DisableAnim();
UFUNCTION(BlueprintCallable)
void DebugUpdatePos();
private: private:
// SO100 Controls by name // SO100 Controls by name
@ -76,7 +79,7 @@ private:
* @return * @return
*/ */
FSo100Actuators GetCurrentControlsFromPhysicScene() const; FSo100Actuators GetCurrentControlsFromPhysicScene() const;
FSo100Actuators GetCurrentJointsFromPhysicScene() const; FSo100Actuators GetCurrentJointsFromPhysicsScene() const;
double GetControlJointDeltaForActuator(FString ActuatorName) const; double GetControlJointDeltaForActuator(FString ActuatorName) const;
static float GetDeltaSumBetweenActuatorValues(const FSo100Actuators& A, const FSo100Actuators& B); static float GetDeltaSumBetweenActuatorValues(const FSo100Actuators& A, const FSo100Actuators& B);
static FSo100Actuators LerpActuators(const FSo100Actuators& A, const FSo100Actuators& B, const float Alpha); static FSo100Actuators LerpActuators(const FSo100Actuators& A, const FSo100Actuators& B, const float Alpha);

2
Source/LuckyWorldV2/Public/Robot/RobotPawn.h
View File

@ -25,7 +25,7 @@ public:
ERobotsName RobotType = ERobotsName::None; // This value must be set in the pawn 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 UPROPERTY(EditAnywhere, BlueprintReadWrite) // TODO Remove UPROPERTY once we migrate physics proxy initialization from Pawn
TObjectPtr<AMujocoVolumeActor> PhysicSceneProxy; TObjectPtr<AMujocoVolumeActor> PhysicsSceneProxy;
UPROPERTY(EditAnywhere, BlueprintReadWrite) // TODO Remove UPROPERTY once we migrate physics proxy initialization from Pawn 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... TObjectPtr<AActor> RobotActor; // My brain is bleeding facing the fact that we have 2 actors...