diff --git a/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoVolumeActor.cpp b/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoVolumeActor.cpp index 1e6fdc8c..1d5b67bd 100644 --- a/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoVolumeActor.cpp +++ b/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoVolumeActor.cpp @@ -115,7 +115,7 @@ void AMujocoVolumeActor::BindPostPhysicDelegate(UserClass* Object, void(UserClas PostPhysicUpdateDelegate.BindUObject(Object, Func); } -void AMujocoVolumeActor::InitializeMujoco() +void AMujocoVolumeActor::InitializeMujoco(TMap ContactExclusion) { if (!Options) { @@ -155,7 +155,7 @@ void AMujocoVolumeActor::InitializeMujoco() } TUniquePtr Generator = MakeUnique(); - TUniquePtr Doc = Generator->GenerateMujocoXml(Options, Objects, ExportFilename); + TUniquePtr Doc = Generator->GenerateMujocoXml(Options, Objects, ExportFilename, ContactExclusion); FString XmlString; tinyxml2::XMLPrinter Printer; @@ -403,14 +403,10 @@ void AMujocoVolumeActor::Tick(float DeltaTime) { if (MujocoData) { - // TODO rename FrameSkip -> SimStepsPerGameFrame - Min 1 - Default 16 - // TODO timestep default .001 (1ms) - // TODO mj_step only inside the for loop - - mj_step(MujocoModel.Get(), MujocoData.Get()); - PostPhysicUpdateDelegate.ExecuteIfBound(MujocoData->time); - - for (int32 Frame = 0; Frame < FrameSkip; ++Frame) + // Default SimStepsPerGameFrame = 16 x 1ms for roughly 60FPS + // If ticks are strictly 16.666 ms then simulation will lag behind UE clock + // Anyway, we need a proper synchronisation between UE and Sim times - probably using TempoTime? + for (int32 SimStep = 0; SimStep < SimStepsPerGameFrame; ++SimStep) { mj_step(MujocoModel.Get(), MujocoData.Get()); PostPhysicUpdateDelegate.ExecuteIfBound(MujocoData->time); diff --git a/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Misc/MujocoXmlGenerator.cpp b/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Misc/MujocoXmlGenerator.cpp index 57336384..8974a736 100644 --- a/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Misc/MujocoXmlGenerator.cpp +++ b/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Misc/MujocoXmlGenerator.cpp @@ -1255,7 +1255,11 @@ bool FMujocoXmlGenerator::ParseActorAsset(AActor* Actor, tinyxml2::XMLElement* R return true; } -TUniquePtr FMujocoXmlGenerator::GenerateMujocoXml(const TObjectPtr& ExportOptions, const TArray& Objects, const FString& ExportFilename) +TUniquePtr FMujocoXmlGenerator::GenerateMujocoXml( + const TObjectPtr& ExportOptions, + const TArray& Objects, + const FString& ExportFilename, + TMap ContactExclusion) { TUniquePtr Doc = MakeUnique(); Doc->InsertFirstChild(Doc->NewDeclaration("xml version=\"1.0\" encoding=\"utf-8\"")); @@ -1268,6 +1272,30 @@ TUniquePtr FMujocoXmlGenerator::GenerateMujocoXml(const T Root->InsertEndChild(Doc->NewElement("equality")); Root->InsertEndChild(Doc->NewElement("tendon")); Root->InsertEndChild(Doc->NewElement("actuator")); + + // TODO Refacto using property from the MujocoActor aka the robot + // TODO Maybe using a ParentObject as parameter which holds that information? + // TODO Need actor refactoring anyway - Merge Pawn and Mujoco Actor? + if (!ContactExclusion.IsEmpty()) + { + Root->InsertEndChild(Doc->NewElement("contact")); + + for (auto& [Body1, Body2] : ContactExclusion) + { + // Don't add empty strings to the map please! + if (Body1.IsEmpty() || Body2.IsEmpty()) continue; + + // Create the contact exclusion - formatted as (SO100 example) + // + const auto Exclude = Doc->NewElement("exclude"); + Exclude->SetAttribute("body1", TCHAR_TO_ANSI(*Body1)); + Exclude->SetAttribute("body2", TCHAR_TO_ANSI(*Body2)); + + // Add exclusion to the contact tag + Root->FirstChildElement("contact")->InsertEndChild(Exclude); + } + } + if (ExportOptions->bAddSkyBox) { tinyxml2::XMLElement* TextureElement = AssetRoot->GetDocument()->NewElement("texture"); diff --git a/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Actors/MujocoVolumeActor.h b/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Actors/MujocoVolumeActor.h index cdbe2cc1..e8a54d1b 100644 --- a/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Actors/MujocoVolumeActor.h +++ b/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Actors/MujocoVolumeActor.h @@ -30,6 +30,27 @@ class LUCKYMUJOCO_API AMujocoVolumeActor : public AActor { GENERATED_BODY() +public: + AMujocoVolumeActor(); + + UFUNCTION(BlueprintCallable, Category = "Mujoco") + + /** + * Initialize the sim scene in headless mujoco + * @param ContactExclusion a list of pairs that should be patched in the xml file for contact exclusion (no friction, no collision) + * TODO ContactExclusion should be stored as a property of MujocoActor and not passed in the scene init + * TODO This require to cast the Actor in addition to the components list + */ + void InitializeMujoco(TMap ContactExclusion); + +protected: + virtual void BeginPlay() override; + virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + virtual void PostRegisterAllComponents() override; + virtual void Tick(float DeltaTime) override; + virtual void PostInitializeComponents() override; + +private: TMujocoModelPtr MujocoModel; TMujocoDataPtr MujocoData; @@ -54,24 +75,11 @@ class LUCKYMUJOCO_API AMujocoVolumeActor : public AActor UPROPERTY(Transient, VisibleAnywhere, Category = "Mujoco | Debug") TArray> TendonComponents; - UFUNCTION(BlueprintCallable, Category = "Mujoco") - void InitializeMujoco(); - template void AssignComponentsToArray(UWorld* World, TArray>& ComponentArray); public: - AMujocoVolumeActor(); - -protected: - virtual void BeginPlay() override; - virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; - virtual void PostRegisterAllComponents() override; - virtual void Tick(float DeltaTime) override; - virtual void PostInitializeComponents() override; - -public: - UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Mujoco | Simulation", meta = (Min = 0, Max = 100, ClampMin = 0, ClampMax = 100)) - int32 FrameSkip = 0; + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Mujoco | Simulation", meta = (Min = 1, Max = 100, ClampMin = 0, ClampMax = 100)) + int32 SimStepsPerGameFrame = 16; UPROPERTY(EditDefaultsOnly, BlueprintReadWrite, Category = "Mujoco") TObjectPtr SpriteComponent; @@ -89,7 +97,7 @@ public: FOnMujocoCompileSuccess OnMujocoCompileSuccess; /** - * @description + * Access MuJoCo scene options and data (e.g. Simulation time) - This is mutable, be careful * @return mjData_ - Full access to mujoco scene options and data */ mjData_& GetMujocoData() const; @@ -101,8 +109,9 @@ private: FPostPhysicUpdate PostPhysicUpdateDelegate; public: /** - * @description Register a delegate to be executed after mj_step - * @param Object + * Register a delegate to be executed after mj_step, useful to fine control actuators + * @param Object - The Class to call + * @param Func - The Function to call - takes a float as parameter which is the current simulation timestamp */ template void BindPostPhysicDelegate(UserClass* Object, void (UserClass::*Func)(float)); diff --git a/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Misc/MujocoOptions.h b/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Misc/MujocoOptions.h index e4fbc49c..7516f428 100644 --- a/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Misc/MujocoOptions.h +++ b/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Misc/MujocoOptions.h @@ -313,7 +313,7 @@ struct LUCKYMUJOCO_API FMujocoOptions /** Simulation time step in seconds. */ UPROPERTY(config, EditAnywhere, BlueprintReadWrite, Category = "Mujoco", meta = (Attribute = "timestep", EditCondition = "bOverrideTimeStep", ClampMin = 0.0001, ClampMax = 0.01, UIMin = 0.0001, UIMax = 0.01)) - float TimeStep = 0.002f; + float TimeStep = 0.001f; // Default to 1ms /** Override API Rate setting. */ UPROPERTY(config, EditAnywhere, BlueprintReadWrite, Category = "Mujoco", meta = (InlineEditConditionToggle)) diff --git a/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Misc/MujocoXmlGenerator.h b/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Misc/MujocoXmlGenerator.h index 7677a38d..f8adf00c 100644 --- a/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Misc/MujocoXmlGenerator.h +++ b/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Misc/MujocoXmlGenerator.h @@ -35,6 +35,19 @@ class LUCKYMUJOCO_API FMujocoXmlGenerator TMap> ObjectMap; -public: - TUniquePtr GenerateMujocoXml(const TObjectPtr& ExportOptions, const TArray& Objects, const FString& ExportFilename); +public: + + /** + * Generate the XML file used by the MuJoCo headless scene representing the physics simulation + * @param ExportOptions TODO - What they can be? + * @param Objects Actors and Components to be added to the scene + * @param ExportFilename pretty much that + * @param ContactExclusion a list of pairs that should be patched in the xml file for contact exclusion (no friction, no collision) + * TODO ContactExclusion should be stored as a property of MujocoActor and not passed in the scene init + */ + TUniquePtr GenerateMujocoXml( + const TObjectPtr& ExportOptions, + const TArray& Objects, + const FString& ExportFilename, + TMap ContactExclusion); }; \ No newline at end of file diff --git a/Plugins/LuckyMujoco/Source/LuckyMujocoEditor/Private/MujocoExporter.cpp b/Plugins/LuckyMujoco/Source/LuckyMujocoEditor/Private/MujocoExporter.cpp index 31408a71..7f1795f6 100644 --- a/Plugins/LuckyMujoco/Source/LuckyMujocoEditor/Private/MujocoExporter.cpp +++ b/Plugins/LuckyMujoco/Source/LuckyMujocoEditor/Private/MujocoExporter.cpp @@ -141,8 +141,8 @@ public: CancelButton->SetEnabled(false); RequestDestroyWindow(); - TUniquePtr Generator = MakeUnique(); - TUniquePtr Doc = Generator->GenerateMujocoXml(ExportOptions, Objects, ExportFilename); + const TUniquePtr Generator = MakeUnique(); + const TUniquePtr Doc = Generator->GenerateMujocoXml(ExportOptions, Objects, ExportFilename, TMap{}); FString XmlString; tinyxml2::XMLPrinter Printer;