FT - Contact Exclusion list for XML scene creation

+ This feature was missing from the plugin while being necessary for robots to work
+ e.g. SO100 requires the main body and the first arm to have contact exclusion, I guess the piece are too tight in the model?
+ This is not clean, but making it right requires to have a better understanding of how the MuJoCo actor works - problem for future self

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<FString, FString> ContactExclusion)
 {
 	if (!Options)
 	{
@@ -155,7 +155,7 @@ void AMujocoVolumeActor::InitializeMujoco()
 		}
 
 		TUniquePtr<FMujocoXmlGenerator> Generator = MakeUnique<FMujocoXmlGenerator>();
-		TUniquePtr<tinyxml2::XMLDocument> Doc = Generator->GenerateMujocoXml(Options, Objects, ExportFilename);
+		TUniquePtr<tinyxml2::XMLDocument> 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
+		// Default SimStepsPerGameFrame = 16 x 1ms for roughly 60FPS
-		// TODO timestep default .001 (1ms)
+		// If ticks are strictly 16.666 ms then simulation will lag behind UE clock
-		// TODO mj_step only inside the for loop
+		// 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 Frame = 0; Frame < FrameSkip; ++Frame)
 		{
 			mj_step(MujocoModel.Get(), MujocoData.Get());
 			PostPhysicUpdateDelegate.ExecuteIfBound(MujocoData->time);

Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Misc/MujocoXmlGenerator.cpp

@@ -1255,7 +1255,11 @@ bool FMujocoXmlGenerator::ParseActorAsset(AActor* Actor, tinyxml2::XMLElement* R
 	return true;
 }
 
-TUniquePtr<tinyxml2::XMLDocument> FMujocoXmlGenerator::GenerateMujocoXml(const TObjectPtr<UMujocoExportOptions>& ExportOptions, const TArray<UObject*>& Objects, const FString& ExportFilename)
+TUniquePtr<tinyxml2::XMLDocument> FMujocoXmlGenerator::GenerateMujocoXml(
+	const TObjectPtr<UMujocoExportOptions>& ExportOptions,
+	const TArray<UObject*>& Objects,
+	const FString& ExportFilename,
+	TMap<FString, FString> ContactExclusion)
 {
 	TUniquePtr<tinyxml2::XMLDocument> Doc = MakeUnique<tinyxml2::XMLDocument>();
 	Doc->InsertFirstChild(Doc->NewDeclaration("xml version=\"1.0\" encoding=\"utf-8\""));
@@ -1268,6 +1272,30 @@ TUniquePtr<tinyxml2::XMLDocument> 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)
+			// <exclude body1="Body_Base" body2="Body_Rotation_Pitch"/> 
+			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");

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<FString, FString> 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<TSoftObjectPtr<UMujocoTendonComponent>> TendonComponents;	
 
-	UFUNCTION(BlueprintCallable, Category = "Mujoco")
-	void InitializeMujoco();
-
 	template <typename ComponentType> void AssignComponentsToArray(UWorld* World, TArray<TSoftObjectPtr<ComponentType>>& ComponentArray);
 
 public:
-	AMujocoVolumeActor();
+	UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Mujoco | Simulation", meta = (Min = 1, Max = 100, ClampMin = 0, ClampMax = 100))
-
+	int32 SimStepsPerGameFrame = 16;
-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(EditDefaultsOnly, BlueprintReadWrite, Category = "Mujoco")
 	TObjectPtr<UBillboardComponent> 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
+	 * Register a delegate to be executed after mj_step, useful to fine control actuators 
-	 * @param Object
+	 * @param Object - The Class to call
+	 * @param Func - The Function to call - takes a float as parameter which is the current simulation timestamp
 	 */
 	template<typename UserClass>
 	void BindPostPhysicDelegate(UserClass* Object, void (UserClass::*Func)(float));

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))

Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Misc/MujocoXmlGenerator.h

@@ -36,5 +36,18 @@ class LUCKYMUJOCO_API FMujocoXmlGenerator
 	TMap<FString, TSoftObjectPtr<UObject>> ObjectMap;
 
 public:
-	TUniquePtr<tinyxml2::XMLDocument> GenerateMujocoXml(const TObjectPtr<UMujocoExportOptions>& ExportOptions, const TArray<UObject*>& Objects, const FString& ExportFilename);
+
+	/**
+	 * 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<tinyxml2::XMLDocument> GenerateMujocoXml(
+		const TObjectPtr<UMujocoExportOptions>& ExportOptions,
+		const TArray<UObject*>& Objects,
+		const FString& ExportFilename,
+		TMap<FString, FString> ContactExclusion);
 };

Plugins/LuckyMujoco/Source/LuckyMujocoEditor/Private/MujocoExporter.cpp

@@ -141,8 +141,8 @@ public:
 			CancelButton->SetEnabled(false);
 			RequestDestroyWindow();
 
-			TUniquePtr<FMujocoXmlGenerator> Generator = MakeUnique<FMujocoXmlGenerator>();			
+			const TUniquePtr<FMujocoXmlGenerator> Generator = MakeUnique<FMujocoXmlGenerator>();
-			TUniquePtr<tinyxml2::XMLDocument> Doc = Generator->GenerateMujocoXml(ExportOptions, Objects, ExportFilename);
+			const TUniquePtr<tinyxml2::XMLDocument> Doc = Generator->GenerateMujocoXml(ExportOptions, Objects, ExportFilename, TMap<FString, FString>{});
 
 			FString XmlString;
 			tinyxml2::XMLPrinter Printer;