diff --git a/Content/Blueprint/Mujoco/BP_Mujoco_so_arm.uasset b/Content/Blueprint/Mujoco/BP_Mujoco_so_arm.uasset index a3dd9cd3..37d1ba3f 100644 Binary files a/Content/Blueprint/Mujoco/BP_Mujoco_so_arm.uasset and b/Content/Blueprint/Mujoco/BP_Mujoco_so_arm.uasset differ diff --git a/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset b/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset index 4ad3a631..a11657d3 100644 Binary files a/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset and b/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset differ diff --git a/Content/Developers/Wdev/Robots/BP_SoArm100robot.uasset b/Content/Developers/Wdev/Robots/BP_SoArm100robot.uasset index 35b89450..61e79171 100644 Binary files a/Content/Developers/Wdev/Robots/BP_SoArm100robot.uasset and b/Content/Developers/Wdev/Robots/BP_SoArm100robot.uasset differ diff --git a/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoVolumeActor.cpp b/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoVolumeActor.cpp index 2aa1ba5e..86f7d859 100644 --- a/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoVolumeActor.cpp +++ b/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoVolumeActor.cpp @@ -109,9 +109,21 @@ template void AMujocoVolumeActor::AssignComponentsToArr } } +template +void AMujocoVolumeActor::BindPostPhysicDelegate(UserClass* Object, void(UserClass::* Func)(float)) +{ + PostPhysicUpdateDelegate.BindUObject(Object, Func); +} + + void AMujocoVolumeActor::InitializeMujoco() { - if (!Options) + InitializeMujocoScene_WithContactExclusion(TMap{}); +} + +void AMujocoVolumeActor::InitializeMujocoScene_WithContactExclusion(const TMap& ContactExclusion) +{ + if (!Options) { return; } @@ -149,7 +161,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; @@ -164,6 +176,8 @@ void AMujocoVolumeActor::InitializeMujoco() return; } + // TODO Here we should check for mujoco dll if we are on windows and LOG an error if there is not + gently quit the game + std::array ErrMsg{}; MujocoModel = MakeMujocoModelPtr(mj_loadXML(TCHAR_TO_ANSI(*ExportFilename), nullptr, ErrMsg.data(), ErrMsg.size())); if (!MujocoModel) @@ -196,6 +210,12 @@ void AMujocoVolumeActor::InitializeMujoco() } } +mjData_& AMujocoVolumeActor::GetMujocoData() const +{ + check(MujocoData.IsValid()); + return *MujocoData.Get(); +} + void AMujocoVolumeActor::SetActuatorValue(const FString& ActuatorName, double Value) { if (MujocoModel) @@ -391,11 +411,13 @@ void AMujocoVolumeActor::Tick(float DeltaTime) { if (MujocoData) { - mj_step(MujocoModel.Get(), MujocoData.Get()); - - 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); } for (int32 i = 1; i < BodyComponents.Num(); ++i) 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 12b041e8..19c91525 100644 --- a/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Actors/MujocoVolumeActor.h +++ b/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Actors/MujocoVolumeActor.h @@ -21,11 +21,44 @@ DECLARE_DYNAMIC_MULTICAST_DELEGATE(FOnMujocoCompileBegin); DECLARE_DYNAMIC_MULTICAST_DELEGATE_OneParam(FOnMujocoCompileError, FString, Error); DECLARE_DYNAMIC_MULTICAST_DELEGATE(FOnMujocoCompileSuccess); +// To be called after mj_step +DECLARE_DELEGATE_OneParam(FPostPhysicUpdate, float); + + UCLASS(Blueprintable, BlueprintType) class LUCKYMUJOCO_API AMujocoVolumeActor : public AActor { GENERATED_BODY() +public: + AMujocoVolumeActor(); + + + /** + * Initialize the sim scene in headless mujoco + */ + UFUNCTION(BlueprintCallable, Category = "Mujoco") + void InitializeMujoco(); + + + /** + * Initialize the sim scene in headless mujoco with a list of contact exclusion + * @param ContactExclusion a list of pairs that should be patched in the xml file for contact exclusion (no friction, no collision) + * TODO Can't use a default empty map as parameter in blueprints? We shouldn't need to have 2 functions + * 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 + */ + UFUNCTION(BlueprintCallable, Category = "Mujoco") + void InitializeMujocoScene_WithContactExclusion(const 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; @@ -50,16 +83,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(); - - 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; @@ -76,6 +104,30 @@ public: UPROPERTY(BlueprintAssignable, Category = "Mujoco | Events") FOnMujocoCompileSuccess OnMujocoCompileSuccess; + /** + * 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; + + // --------------------------- + // ------- POST UPDATE ------- + // --------------------------- +private: + FPostPhysicUpdate PostPhysicUpdateDelegate; +public: + /** + * 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)); + + + // ------------------------- + // ------- ACTUATORS ------- + // ------------------------- UFUNCTION(BlueprintCallable, Category = "Mujoco") void SetActuatorValue(const FString& ActuatorName, double Value); @@ -94,6 +146,9 @@ public: UFUNCTION(BlueprintCallable, BlueprintPure, Category = "Mujoco") FVector2D GetActuatorRangeByIndex(int32 ActuatorIndex) const; + // ---------------------- + // ------- JOINTS ------- + // ---------------------- UFUNCTION(BlueprintCallable, Category = "Mujoco") void SetJointValue(const FString& JointName, double Value); @@ -105,16 +160,4 @@ public: UFUNCTION(BlueprintCallable, BlueprintPure, Category = "Mujoco") double GetJointValueByIndex(int32 JointIndex) const; - - - virtual void PostRegisterAllComponents() override; - - virtual void Tick(float DeltaTime) override; - - virtual void PostInitializeComponents() override; - -protected: - - virtual void BeginPlay() override; - virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; }; diff --git a/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Misc/MujocoOptions.h b/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Misc/MujocoOptions.h index e4fbc49c..6b30c6b7 100644 --- a/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Misc/MujocoOptions.h +++ b/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Misc/MujocoOptions.h @@ -307,13 +307,13 @@ struct LUCKYMUJOCO_API FMujocoOptions { GENERATED_BODY() - /** Override TimeStep setting. When enabled, the default value (0.002) is applied. */ + /** Override TimeStep setting. When enabled, the default value (0.001) is applied. */ UPROPERTY(config, EditAnywhere, BlueprintReadWrite, Category = "Mujoco", meta = (InlineEditConditionToggle)) bool bOverrideTimeStep = false; /** 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;