diff --git a/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset b/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset index 68616f5b..d9239fe8 100644 Binary files a/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset and b/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset differ diff --git a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp index 1ee82e60..173d3bab 100644 --- a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp +++ b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp @@ -25,9 +25,9 @@ void URobotPilotComponent::TickComponent(float DeltaTime, enum ELevelTick TickTy 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); } } diff --git a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp index f62fbf7c..dc1357ca 100644 --- a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp +++ b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp @@ -21,7 +21,7 @@ void URobotPilotSO100Component::TickComponent(float DeltaTime, enum ELevelTick T FActorComponentTickFunction* ThisTickFunction) { // Super::TickComponent(DeltaTime, TickType, ThisTickFunction); - // const auto Joints = GetCurrentJointsFromPhysicScene(); + // const auto Joints = GetCurrentJointsFromPhysicsScene(); // const auto Controls = GetCurrentControlsFromPhysicScene(); // 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; } +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 { return FSo100Actuators { - RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Rotation), - RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Pitch), - RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Elbow), - RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_WristPitch), - RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_WristRoll), - RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Jaw), + RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Rotation), + RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Pitch), + RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Elbow), + RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_WristPitch), + RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_WristRoll), + 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 // Joints get a "1" appended to their name -> refacto so it's "_Joint" return FSo100Actuators { - RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Rotation).Append("1")), - RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Pitch).Append("1")), - RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Elbow).Append("1")), - RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_WristPitch).Append("1")), - RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_WristRoll).Append("1")), - RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Jaw).Append("1")) + RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Rotation).Append("1")), + RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Pitch).Append("1")), + RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Elbow).Append("1")), + RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_WristPitch).Append("1")), + RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_WristRoll).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 { - auto const Control = RobotOwner->PhysicSceneProxy->GetActuatorValue(ActuatorName); - auto const Joint = RobotOwner->PhysicSceneProxy->GetJointValue(ActuatorName.Append("1")); + auto const Control = RobotOwner->PhysicsSceneProxy->GetActuatorValue(ActuatorName); + auto const Joint = RobotOwner->PhysicsSceneProxy->GetJointValue(ActuatorName.Append("1")); return Control -Joint; } @@ -143,9 +158,9 @@ void URobotPilotSO100Component::PostPhysicStepUpdate(const float SimulationTime) void URobotPilotSO100Component::NextAnimationState() { - const float CurrentSimulationTime = RobotOwner->PhysicSceneProxy->GetMujocoData().time; - AnimationStartTime = CurrentSimulationTime; - AnimStartRobotActuators = GetCurrentJointsFromPhysicScene(); + const float CurrentPhysicsEngineSceneTime = RobotOwner->PhysicsSceneProxy->GetMujocoData().time; + AnimationStartTime = CurrentPhysicsEngineSceneTime; + AnimStartRobotActuators = GetCurrentJointsFromPhysicsScene(); switch (CurrentAnimationState) { @@ -226,7 +241,7 @@ void URobotPilotSO100Component::MoveToTarget() const auto AlphaExtend = FMath::Clamp(Distance / MaxReach, 0., 1.); // Set the target actuators values - AnimTargetRobotActuators = GetCurrentJointsFromPhysicScene(); + AnimTargetRobotActuators = GetCurrentJointsFromPhysicsScene(); AnimTargetRobotActuators = LerpActuators(ActuatorsRestPosition, ActuatorsMaxExtendPosition, AlphaExtend); @@ -268,7 +283,7 @@ void URobotPilotSO100Component::MoveToDropZone() AnimTargetRobotActuators = ActuatorsDropZone; AnimTargetRobotActuators.Rotation = ActuatorsDropZone.Rotation * (FMath::RandBool() ? 1. : -1.); - AnimTargetRobotActuators.Jaw = GetCurrentJointsFromPhysicScene().Jaw; + AnimTargetRobotActuators.Jaw = GetCurrentJointsFromPhysicsScene().Jaw; CurrentAnimationState = 4; AnimationDuration = 3.f; } @@ -287,7 +302,7 @@ 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, GetCurrentJointsFromPhysicScene()); + const auto DeltaSum = GetDeltaSumBetweenActuatorValues(AnimTargetRobotActuators, GetCurrentJointsFromPhysicsScene()); // Alternative Animation completion event - checking for over-current if (bDetectOverCurrent && GetControlJointDeltaForActuator(Actuator_Jaw) > OverCurrentThreshold) @@ -303,42 +318,42 @@ bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime) if (AnimAlpha >= 1. && DeltaSum <= .001) return true; // Rotation - RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Rotation, FMath::Lerp( + RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Rotation, FMath::Lerp( AnimStartRobotActuators.Rotation, AnimTargetRobotActuators.Rotation, AnimAlpha )); // Pitch - RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Pitch, FMath::Lerp( + RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Pitch, FMath::Lerp( AnimStartRobotActuators.Pitch, AnimTargetRobotActuators.Pitch, AnimAlpha )); // Elbow - RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Elbow, FMath::Lerp( + RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Elbow, FMath::Lerp( AnimStartRobotActuators.Elbow, AnimTargetRobotActuators.Elbow, AnimAlpha )); // WristPitch - RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_WristPitch, FMath::Lerp( + RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_WristPitch, FMath::Lerp( AnimStartRobotActuators.WristPitch, AnimTargetRobotActuators.WristPitch, AnimAlpha )); // WristRoll - RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_WristRoll, FMath::Lerp( + RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_WristRoll, FMath::Lerp( AnimStartRobotActuators.WristRoll, AnimTargetRobotActuators.WristRoll, AnimAlpha )); // Jaw - RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Jaw, FMath::Lerp( + RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Jaw, FMath::Lerp( AnimStartRobotActuators.Jaw, AnimTargetRobotActuators.Jaw, AnimAlpha diff --git a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h index 767b6fbc..6e325e23 100644 --- a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h +++ b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h @@ -52,6 +52,9 @@ public: UFUNCTION(BlueprintCallable) void DisableAnim(); + + UFUNCTION(BlueprintCallable) + void DebugUpdatePos(); private: // SO100 Controls by name @@ -76,7 +79,7 @@ private: * @return */ FSo100Actuators GetCurrentControlsFromPhysicScene() const; - FSo100Actuators GetCurrentJointsFromPhysicScene() const; + FSo100Actuators GetCurrentJointsFromPhysicsScene() const; double GetControlJointDeltaForActuator(FString ActuatorName) const; static float GetDeltaSumBetweenActuatorValues(const FSo100Actuators& A, const FSo100Actuators& B); static FSo100Actuators LerpActuators(const FSo100Actuators& A, const FSo100Actuators& B, const float Alpha); diff --git a/Source/LuckyWorldV2/Public/Robot/RobotPawn.h b/Source/LuckyWorldV2/Public/Robot/RobotPawn.h index 45ed1c72..80b5177f 100644 --- a/Source/LuckyWorldV2/Public/Robot/RobotPawn.h +++ b/Source/LuckyWorldV2/Public/Robot/RobotPawn.h @@ -25,7 +25,7 @@ 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 - TObjectPtr PhysicSceneProxy; + TObjectPtr PhysicsSceneProxy; UPROPERTY(EditAnywhere, BlueprintReadWrite) // TODO Remove UPROPERTY once we migrate physics proxy initialization from Pawn TObjectPtr RobotActor; // My brain is bleeding facing the fact that we have 2 actors...