2207cfbba0
+ 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
1386 lines
49 KiB
C++
1386 lines
49 KiB
C++
#include "Misc/MujocoXmlGenerator.h"
|
|
#include "Components/SceneComponent.h"
|
|
#include "Components/StaticMeshComponent.h"
|
|
#include "Components/SkeletalMeshComponent.h"
|
|
#include "Components/PrimitiveComponent.h"
|
|
#include "tinyxml2.h"
|
|
#include "Misc/FileHelper.h"
|
|
#include "Misc/Paths.h"
|
|
#include "Components/MujocoBodyComponent.h"
|
|
#include "Engine/SCS_Node.h"
|
|
#include "StaticMeshOperations.h"
|
|
#include "functional"
|
|
#include "Actors/MujocoStaticMeshActor.h"
|
|
#include "Engine/LevelScriptActor.h"
|
|
#include "Components/MujocoGeomComponent.h"
|
|
#include "Components/MujocoJointComponent.h"
|
|
#include "Components/MujocoSiteComponent.h"
|
|
#include "Components/MujocoActuatorComponent.h"
|
|
#include "Components/MujocoEqualityComponent.h"
|
|
#include "Components/MujocoTendonComponent.h"
|
|
#include "UObject/PropertyOptional.h"
|
|
#include "Structs/MujocoInertial.h"
|
|
#include "Structs/MujocoSite.h"
|
|
#include "Structs/MujocoGeom.h"
|
|
#include "Enums/MujocoEnums.h"
|
|
#include "Camera/CameraComponent.h"
|
|
#include "GameFramework/SpringArmComponent.h"
|
|
#include "Misc/MujocoOptions.h"
|
|
#include "array"
|
|
#include "LuckyMujoco.h"
|
|
|
|
template <typename T> FString GetAttributeMappingForProperty(FProperty* /*Prop*/)
|
|
{
|
|
return FString();
|
|
}
|
|
|
|
template <> FString GetAttributeMappingForProperty<FMujocoTendonFixedJoint>(FProperty* Prop)
|
|
{
|
|
// clang-format off
|
|
constexpr std::array Mapping{
|
|
std::pair{ TEXT("Joint"), TEXT("joint") },
|
|
std::pair{ TEXT("Coef"), TEXT("coef") },
|
|
};
|
|
// clang-format on
|
|
|
|
FString PropName = Prop->GetNameCPP();
|
|
for (const auto& Pair : Mapping)
|
|
{
|
|
if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
|
|
{
|
|
return FString(Pair.second);
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
template <> FString GetAttributeMappingForProperty<FMujocoTendonSpatialSite>(FProperty* Prop)
|
|
{
|
|
// clang-format off
|
|
constexpr std::array Mapping{
|
|
std::pair{ TEXT("Site"), TEXT("site") },
|
|
};
|
|
// clang-format on
|
|
|
|
FString PropName = Prop->GetNameCPP();
|
|
for (const auto& Pair : Mapping)
|
|
{
|
|
if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
|
|
{
|
|
return FString(Pair.second);
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
template <> FString GetAttributeMappingForProperty<FMujocoTendonSpatialGeom>(FProperty* Prop)
|
|
{
|
|
// clang-format off
|
|
constexpr std::array Mapping{
|
|
std::pair{ TEXT("Geom"), TEXT("geom") },
|
|
std::pair{ TEXT("SideSite"), TEXT("sidesite") },
|
|
};
|
|
// clang-format on
|
|
|
|
FString PropName = Prop->GetNameCPP();
|
|
for (const auto& Pair : Mapping)
|
|
{
|
|
if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
|
|
{
|
|
return FString(Pair.second);
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
template <> FString GetAttributeMappingForProperty<FMujocoTendonPulley>(FProperty* Prop)
|
|
{
|
|
// clang-format off
|
|
constexpr std::array Mapping{
|
|
std::pair{ TEXT("Divisor"), TEXT("divisor") },
|
|
};
|
|
// clang-format on
|
|
|
|
FString PropName = Prop->GetNameCPP();
|
|
for (const auto& Pair : Mapping)
|
|
{
|
|
if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
|
|
{
|
|
return FString(Pair.second);
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
template <> FString GetAttributeMappingForProperty<FMujocoSimulationFlags>(FProperty* Prop)
|
|
{
|
|
// clang-format off
|
|
constexpr std::array Mapping{
|
|
std::pair{ TEXT("Constraint"), TEXT("constraint") },
|
|
std::pair{ TEXT("Equality"), TEXT("equality") },
|
|
std::pair{ TEXT("FrictionLoss"), TEXT("frictionloss") },
|
|
std::pair{ TEXT("Limit"), TEXT("limit") },
|
|
std::pair{ TEXT("Contact"), TEXT("contact") },
|
|
std::pair{ TEXT("Passive"), TEXT("passive") },
|
|
std::pair{ TEXT("Gravity"), TEXT("gravity") },
|
|
std::pair{ TEXT("Actuation"), TEXT("actuation") },
|
|
std::pair{ TEXT("RefSafe"), TEXT("refsafe") },
|
|
std::pair{ TEXT("Sensor"), TEXT("sensor") },
|
|
std::pair{ TEXT("Energy"), TEXT("energy") },
|
|
std::pair{ TEXT("FwdInv"), TEXT("fwdinv") },
|
|
std::pair{ TEXT("MultiCCD"), TEXT("multiccd") },
|
|
std::pair{ TEXT("Island"), TEXT("island") },
|
|
std::pair{ TEXT("NativeCCD"), TEXT("nativeccd") }
|
|
};
|
|
// clang-format on
|
|
FString PropName = Prop->GetNameCPP();
|
|
for (const auto& Pair : Mapping)
|
|
{
|
|
if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
|
|
{
|
|
return FString(Pair.second);
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
template <> FString GetAttributeMappingForProperty<FMujocoCompileOptions>(FProperty* Prop)
|
|
{
|
|
// clang-format off
|
|
constexpr std::array Mapping{
|
|
std::pair{ TEXT("AutoLimits"), TEXT("autolimits") },
|
|
std::pair{ TEXT("BoundMass"), TEXT("boundmass") },
|
|
std::pair{ TEXT("BoundInertia"), TEXT("boundinertia") },
|
|
std::pair{ TEXT("SetTotalMass"), TEXT("settotalmass") },
|
|
std::pair{ TEXT("BalanceInertia"), TEXT("balanceinertia") },
|
|
std::pair{ TEXT("StripPath"), TEXT("strippath") },
|
|
std::pair{ TEXT("AngleUnits"), TEXT("angle") },
|
|
std::pair{ TEXT("FitAABB"), TEXT("fitaabb") },
|
|
std::pair{ TEXT("EulerSequence"), TEXT("eulerseq") },
|
|
std::pair{ TEXT("MeshDir"), TEXT("meshdir") },
|
|
std::pair{ TEXT("TextureDir"), TEXT("texturedir") },
|
|
std::pair{ TEXT("AssetDir"), TEXT("assetdir") },
|
|
std::pair{ TEXT("DiscardVisual"), TEXT("discardvisual") },
|
|
std::pair{ TEXT("UseThread"), TEXT("usethread") },
|
|
std::pair{ TEXT("FuseStatic"), TEXT("fusestatic") },
|
|
std::pair{ TEXT("InertiaFromGeom"), TEXT("inertiafromgeom") },
|
|
std::pair{ TEXT("AlignFree"), TEXT("alignfree") },
|
|
std::pair{ TEXT("InertiaGroupRange"), TEXT("inertiagrouprange") }
|
|
};
|
|
// clang-format on
|
|
FString PropName = Prop->GetNameCPP();
|
|
for (const auto& Pair : Mapping)
|
|
{
|
|
if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
|
|
{
|
|
return FString(Pair.second);
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
template <> FString GetAttributeMappingForProperty<FMujocoOptions>(FProperty* Prop)
|
|
{
|
|
// clang-format off
|
|
constexpr std::array Mapping{
|
|
std::pair{ TEXT("TimeStep"), TEXT("timestep") },
|
|
std::pair{ TEXT("ApiRate"), TEXT("apirate") },
|
|
std::pair{ TEXT("ImpRatio"), TEXT("impratio") },
|
|
std::pair{ TEXT("Gravity"), TEXT("gravity") },
|
|
std::pair{ TEXT("Wind"), TEXT("wind") },
|
|
std::pair{ TEXT("Magnetic"), TEXT("magnetic") },
|
|
std::pair{ TEXT("Density"), TEXT("density") },
|
|
std::pair{ TEXT("Viscosity"), TEXT("viscosity") },
|
|
std::pair{ TEXT("MarginOverride"), TEXT("o_margin") },
|
|
std::pair{ TEXT("Integrator"), TEXT("integrator") },
|
|
std::pair{ TEXT("ConeType"), TEXT("cone") },
|
|
std::pair{ TEXT("JacobianType"), TEXT("jacobian") },
|
|
std::pair{ TEXT("Solver"), TEXT("solver") },
|
|
std::pair{ TEXT("MaxIterations"), TEXT("iterations") },
|
|
std::pair{ TEXT("SolverTolerance"), TEXT("tolerance") },
|
|
std::pair{ TEXT("LineSearchIterations"), TEXT("ls_iterations") },
|
|
std::pair{ TEXT("LineSearchTolerance"), TEXT("ls_tolerance") },
|
|
std::pair{ TEXT("NoSlipIterations"), TEXT("noslip_iterations") },
|
|
std::pair{ TEXT("NoSlipTolerance"), TEXT("noslip_tolerance") },
|
|
std::pair{ TEXT("CCDIterations"), TEXT("ccd_iterations") },
|
|
std::pair{ TEXT("CCDTolerance"), TEXT("ccd_tolerance") },
|
|
std::pair{ TEXT("SDFIterations"), TEXT("sdf_iterations") },
|
|
std::pair{ TEXT("SDFInitPoints"), TEXT("sdf_initpoints") },
|
|
std::pair{ TEXT("ActuatorGroupDisable"), TEXT("actuatorgroupdisable") }
|
|
};
|
|
// clang-format on
|
|
FString PropName = Prop->GetNameCPP();
|
|
for (const auto& Pair : Mapping)
|
|
{
|
|
if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
|
|
{
|
|
return FString(Pair.second);
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
template <> FString GetAttributeMappingForProperty<FMujocoActuatorV2>(FProperty* Prop)
|
|
{
|
|
// clang-format off
|
|
constexpr std::array Mapping{
|
|
std::pair{ TEXT("Group"), TEXT("group") },
|
|
std::pair{ TEXT("CtrlLimited"), TEXT("ctrllimited") },
|
|
std::pair{ TEXT("ForceLimited"), TEXT("forcelimited") },
|
|
std::pair{ TEXT("ActLimited"), TEXT("actlimited") },
|
|
std::pair{ TEXT("CtrlRange"), TEXT("ctrlrange") },
|
|
std::pair{ TEXT("ForceRange"), TEXT("forcerange") },
|
|
std::pair{ TEXT("ActRange"), TEXT("actrange") },
|
|
std::pair{ TEXT("LengthRange"), TEXT("lengthrange") },
|
|
std::pair{ TEXT("Gear"), TEXT("gear") },
|
|
std::pair{ TEXT("CrankLength"), TEXT("cranklength") },
|
|
std::pair{ TEXT("Joint"), TEXT("joint") },
|
|
std::pair{ TEXT("JointInParent"), TEXT("jointinparent") },
|
|
std::pair{ TEXT("Site"), TEXT("site") },
|
|
std::pair{ TEXT("RefSite"), TEXT("refsite") },
|
|
std::pair{ TEXT("Body"), TEXT("body") },
|
|
std::pair{ TEXT("Tendon"), TEXT("tendon") },
|
|
std::pair{ TEXT("CrankSite"), TEXT("cranksite") },
|
|
std::pair{ TEXT("SliderSite"), TEXT("slidersite") },
|
|
std::pair{ TEXT("Kp"), TEXT("kp") },
|
|
std::pair{ TEXT("Kv"), TEXT("kv") },
|
|
std::pair{ TEXT("DampingRatio"), TEXT("dampratio") },
|
|
std::pair{ TEXT("TimeConst"), TEXT("timeconst") },
|
|
std::pair{ TEXT("InheritRange"), TEXT("inheritrange") },
|
|
std::pair{ TEXT("Area"), TEXT("area") },
|
|
std::pair{ TEXT("Diameter"), TEXT("diameter") },
|
|
std::pair{ TEXT("Bias"), TEXT("bias") },
|
|
std::pair{ TEXT("MuscleTimeConsts"), TEXT("timeconst") },
|
|
std::pair{ TEXT("TauSmooth"), TEXT("tausmooth") },
|
|
std::pair{ TEXT("Range"), TEXT("range") },
|
|
std::pair{ TEXT("Force"), TEXT("force") },
|
|
std::pair{ TEXT("Scale"), TEXT("scale") },
|
|
std::pair{ TEXT("VMax"), TEXT("vmax") },
|
|
std::pair{ TEXT("FpMax"), TEXT("fpmax") },
|
|
std::pair{ TEXT("FvMax"), TEXT("fvmax") },
|
|
std::pair{ TEXT("LMin"), TEXT("lmin") },
|
|
std::pair{ TEXT("LMax"), TEXT("lmax") },
|
|
std::pair{ TEXT("AdhesionGain"), TEXT("gain") },
|
|
std::pair{ TEXT("DynType"), TEXT("dyntype") },
|
|
std::pair{ TEXT("GainType"), TEXT("gaintype") },
|
|
std::pair{ TEXT("BiasType"), TEXT("biastype") },
|
|
std::pair{ TEXT("DynPrm"), TEXT("dynprm") },
|
|
std::pair{ TEXT("GainPrm"), TEXT("gainprm") },
|
|
std::pair{ TEXT("BiasPrm"), TEXT("biasprm") },
|
|
std::pair{ TEXT("ActEarly"), TEXT("actearly") },
|
|
};
|
|
// clang-format on
|
|
FString PropName = Prop->GetNameCPP();
|
|
for (const auto& Pair : Mapping)
|
|
{
|
|
if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
|
|
{
|
|
return FString(Pair.second);
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
template <> FString GetAttributeMappingForProperty<FMujocoEquality>(FProperty* Prop)
|
|
{
|
|
// clang-format off
|
|
constexpr std::array Mapping{
|
|
std::pair{ TEXT("Active"), TEXT("active") },
|
|
std::pair{ TEXT("Body1"), TEXT("body1") },
|
|
std::pair{ TEXT("Body2"), TEXT("body2") },
|
|
std::pair{ TEXT("Site1"), TEXT("site1") },
|
|
std::pair{ TEXT("Site2"), TEXT("site2") },
|
|
std::pair{ TEXT("Joint1"), TEXT("joint1") },
|
|
std::pair{ TEXT("Joint2"), TEXT("joint2") },
|
|
std::pair{ TEXT("Tendon1"), TEXT("tendon1") },
|
|
std::pair{ TEXT("Tendon2"), TEXT("tendon2") },
|
|
std::pair{ TEXT("Flex"), TEXT("flex") },
|
|
std::pair{ TEXT("SolRef"), TEXT("solref") },
|
|
std::pair{ TEXT("SolImp"), TEXT("solimp") },
|
|
std::pair{ TEXT("Anchor"), TEXT("anchor") },
|
|
std::pair{ TEXT("RelPose"), TEXT("relpose") },
|
|
std::pair{ TEXT("TorqueScale"), TEXT("torquescale") },
|
|
std::pair{ TEXT("PolyCoef"), TEXT("polycoef") }
|
|
};
|
|
// clang-format on
|
|
FString PropName = Prop->GetNameCPP();
|
|
for (const auto& Pair : Mapping)
|
|
{
|
|
if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
|
|
{
|
|
return FString(Pair.second);
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
template <> FString GetAttributeMappingForProperty<FMujocoGeom>(FProperty* Prop)
|
|
{
|
|
// clang-format off
|
|
constexpr std::array Mapping{
|
|
std::pair{ TEXT("ConType"), TEXT("contype") },
|
|
std::pair{ TEXT("ConAffinity"), TEXT("conaffinity") },
|
|
std::pair{ TEXT("ConDim"), TEXT("condim") },
|
|
std::pair{ TEXT("Group"), TEXT("group") },
|
|
std::pair{ TEXT("Priority"), TEXT("priority") },
|
|
std::pair{ TEXT("Friction"), TEXT("friction") },
|
|
std::pair{ TEXT("Mass"), TEXT("mass") },
|
|
std::pair{ TEXT("Density"), TEXT("density") },
|
|
std::pair{ TEXT("ShellInertia"), TEXT("shellinertia") },
|
|
std::pair{ TEXT("Solmix"), TEXT("solmix") },
|
|
std::pair{ TEXT("SolImp"), TEXT("solimp") },
|
|
std::pair{ TEXT("SolRef"), TEXT("solref") },
|
|
std::pair{ TEXT("Margin"), TEXT("margin") },
|
|
std::pair{ TEXT("Gap"), TEXT("gap") }
|
|
};
|
|
// clang-format on
|
|
FString PropName = Prop->GetNameCPP();
|
|
for (const auto& Pair : Mapping)
|
|
{
|
|
if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
|
|
{
|
|
return FString(Pair.second);
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
template <> FString GetAttributeMappingForProperty<FMujocoInertial>(FProperty* Prop)
|
|
{
|
|
// clang-format off
|
|
constexpr std::array Mapping{
|
|
std::pair{ TEXT("Pos"), TEXT("pos") },
|
|
std::pair{ TEXT("Mass"), TEXT("mass") },
|
|
std::pair{ TEXT("DiagInertia"), TEXT("diaginertia") },
|
|
std::pair{ TEXT("FullInertia"), TEXT("fullinertia") }
|
|
};
|
|
// clang-format on
|
|
FString PropName = Prop->GetNameCPP();
|
|
for (const auto& Pair : Mapping)
|
|
{
|
|
if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
|
|
{
|
|
return FString(Pair.second);
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
template <> FString GetAttributeMappingForProperty<FMujocoJoint>(FProperty* Prop)
|
|
{
|
|
// clang-format off
|
|
constexpr std::array Mapping{
|
|
std::pair{ TEXT("Type"), TEXT("type") },
|
|
std::pair{ TEXT("Group"), TEXT("group") },
|
|
std::pair{ TEXT("Position"), TEXT("pos") },
|
|
std::pair{ TEXT("Axis"), TEXT("axis") },
|
|
std::pair{ TEXT("SpringDamper"), TEXT("springdamper") },
|
|
std::pair{ TEXT("Stiffness"), TEXT("stiffness") },
|
|
std::pair{ TEXT("Range"), TEXT("range") },
|
|
std::pair{ TEXT("LimitType"), TEXT("limited") },
|
|
std::pair{ TEXT("ActuatorForceRange"), TEXT("actuatorfrcrange") },
|
|
std::pair{ TEXT("ActuatorForceLimitType"), TEXT("actuatorfrclimited") },
|
|
std::pair{ TEXT("bActuatorGravityCompensation"), TEXT("actuatorgravcomp") },
|
|
std::pair{ TEXT("Margin"), TEXT("margin") },
|
|
std::pair{ TEXT("Reference"), TEXT("ref") },
|
|
std::pair{ TEXT("SpringReference"), TEXT("springref") },
|
|
std::pair{ TEXT("Armature"), TEXT("armature") },
|
|
std::pair{ TEXT("Damping"), TEXT("damping") },
|
|
std::pair{ TEXT("FrictionLoss"), TEXT("frictionloss") }
|
|
};
|
|
// clang-format on
|
|
FString PropName = Prop->GetNameCPP();
|
|
for (const auto& Pair : Mapping)
|
|
{
|
|
if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
|
|
{
|
|
return FString(Pair.second);
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
template <> FString GetAttributeMappingForProperty<FMujocoSite>(FProperty* Prop)
|
|
{
|
|
// clang-format off
|
|
constexpr std::array Mapping{
|
|
std::pair{ TEXT("Type"), TEXT("type") },
|
|
std::pair{ TEXT("Group"), TEXT("group") },
|
|
std::pair{ TEXT("Rgba"), TEXT("rgba") },
|
|
std::pair{ TEXT("Size"), TEXT("size") }
|
|
};
|
|
// clang-format on
|
|
FString PropName = Prop->GetNameCPP();
|
|
for (const auto& Pair : Mapping)
|
|
{
|
|
if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
|
|
{
|
|
return FString(Pair.second);
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
template <> FString GetAttributeMappingForProperty<FMujocoTendon>(FProperty* Prop)
|
|
{
|
|
// clang-format off
|
|
constexpr std::array Mapping{
|
|
std::pair{ TEXT("Name"), TEXT("name") },
|
|
std::pair{ TEXT("Group"), TEXT("group") },
|
|
std::pair{ TEXT("Limited"), TEXT("limited") },
|
|
std::pair{ TEXT("Range"), TEXT("range") },
|
|
std::pair{ TEXT("SolRefLimit"), TEXT("solreflimit") },
|
|
std::pair{ TEXT("SolImpLimit"), TEXT("solimplimit") },
|
|
std::pair{ TEXT("SolRefFriction"), TEXT("solreffriction") },
|
|
std::pair{ TEXT("SolImpFriction"), TEXT("solimpfriction") },
|
|
std::pair{ TEXT("Margin"), TEXT("margin") },
|
|
std::pair{ TEXT("FrictionLoss"), TEXT("frictionloss") },
|
|
std::pair{ TEXT("Width"), TEXT("width") },
|
|
std::pair{ TEXT("SpringLength"), TEXT("springlength") },
|
|
std::pair{ TEXT("Stiffness"), TEXT("stiffness") },
|
|
std::pair{ TEXT("Damping"), TEXT("damping") },
|
|
std::pair{ TEXT("SpatialSite"), TEXT("site") },
|
|
std::pair{ TEXT("SpatialGeom"), TEXT("geom") },
|
|
std::pair{ TEXT("Pulley"), TEXT("pulley") },
|
|
std::pair{ TEXT("FixedJoint"), TEXT("fixed") }
|
|
};
|
|
// clang-format on
|
|
FString PropName = Prop->GetNameCPP();
|
|
for (const auto& Pair : Mapping)
|
|
{
|
|
if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
|
|
{
|
|
return FString(Pair.second);
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
template <typename T> FString GetStructToString(FStructProperty* StructProperty, void* Container)
|
|
{
|
|
if (StructProperty->Struct->GetFName() == T::StaticStruct()->GetFName())
|
|
{
|
|
T* StructInstance = StructProperty->ContainerPtrToValuePtr<T>(Container);
|
|
if (StructInstance)
|
|
{
|
|
return StructInstance->ToString();
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
const TSet<FString> NotLowerCase = { "Euler", "RK4", "PGS", "CG", "Newton" };
|
|
FString GetPropertyValueAsString(FProperty* Property, void* Container)
|
|
{
|
|
FString ValueStr;
|
|
if (FEnumProperty* EnumProperty = CastField<FEnumProperty>(Property))
|
|
{
|
|
int64 EnumValue = EnumProperty->GetUnderlyingProperty()->GetSignedIntPropertyValue(EnumProperty->ContainerPtrToValuePtr<void>(Container));
|
|
auto DisplayText = EnumProperty->GetEnum()->GetNameStringByValue(EnumValue);
|
|
|
|
if (!NotLowerCase.Contains(DisplayText))
|
|
{
|
|
ValueStr = DisplayText.ToLower();
|
|
}
|
|
else
|
|
{
|
|
ValueStr = DisplayText;
|
|
}
|
|
|
|
if (Property->GetFName() == GET_MEMBER_NAME_CHECKED(FMujocoGeom, ConDim))
|
|
{
|
|
ValueStr = FString::FromInt(EnumValue);
|
|
}
|
|
else if (ValueStr.EndsWith("_"))
|
|
{
|
|
ValueStr = ValueStr.LeftChop(1);
|
|
}
|
|
}
|
|
else if (FArrayProperty* NumericArrayProperty = CastField<FArrayProperty>(Property))
|
|
{
|
|
FProperty* InnerProperty = NumericArrayProperty->Inner;
|
|
FScriptArrayHelper ArrayHelper(NumericArrayProperty, NumericArrayProperty->ContainerPtrToValuePtr<void>(Container));
|
|
|
|
if (InnerProperty->IsA<FNumericProperty>() && CastField<FNumericProperty>(InnerProperty)->IsFloatingPoint())
|
|
{
|
|
TArray<FString> Values;
|
|
for (int32 i = 0; i < ArrayHelper.Num(); ++i)
|
|
{
|
|
double Value = CastField<FNumericProperty>(InnerProperty)->GetFloatingPointPropertyValue(ArrayHelper.GetRawPtr(i));
|
|
Values.Add(FString::SanitizeFloat(Value));
|
|
}
|
|
ValueStr = FString::Join(Values, TEXT(" "));
|
|
}
|
|
else if (InnerProperty->IsA<FNumericProperty>() && CastField<FNumericProperty>(InnerProperty)->IsInteger())
|
|
{
|
|
TArray<FString> Values;
|
|
for (int32 i = 0; i < ArrayHelper.Num(); ++i)
|
|
{
|
|
int64 Value = CastField<FNumericProperty>(InnerProperty)->GetSignedIntPropertyValue(ArrayHelper.GetRawPtr(i));
|
|
Values.Add(FString::Printf(TEXT("%lld"), Value));
|
|
}
|
|
ValueStr = FString::Join(Values, TEXT(" "));
|
|
}
|
|
}
|
|
else if (FNumericProperty* NumericProperty = CastField<FNumericProperty>(Property))
|
|
{
|
|
if (NumericProperty->IsFloatingPoint())
|
|
{
|
|
double Value = NumericProperty->GetFloatingPointPropertyValue(NumericProperty->ContainerPtrToValuePtr<void>(Container));
|
|
ValueStr = FString::SanitizeFloat(Value);
|
|
}
|
|
else if (NumericProperty->IsInteger())
|
|
{
|
|
int64 Value = NumericProperty->GetSignedIntPropertyValue(NumericProperty->ContainerPtrToValuePtr<void>(Container));
|
|
ValueStr = FString::Printf(TEXT("%lld"), Value);
|
|
}
|
|
}
|
|
else if (FBoolProperty* BoolProperty = CastField<FBoolProperty>(Property))
|
|
{
|
|
bool Value = BoolProperty->GetPropertyValue(BoolProperty->ContainerPtrToValuePtr<void>(Container));
|
|
ValueStr = Value ? TEXT("true") : TEXT("false");
|
|
}
|
|
else if (FStrProperty* StringProperty = CastField<FStrProperty>(Property))
|
|
{
|
|
FString* String = StringProperty->ContainerPtrToValuePtr<FString>(Container);
|
|
if (String && !String->IsEmpty())
|
|
{
|
|
ValueStr = StringProperty->GetPropertyValue(String);
|
|
}
|
|
}
|
|
else if (FNameProperty* NameProperty = CastField<FNameProperty>(Property))
|
|
{
|
|
auto* Name = NameProperty->ContainerPtrToValuePtr<FName>(Container);
|
|
if (Name && !Name->IsNone())
|
|
{
|
|
ValueStr = NameProperty->GetPropertyValue(Name).ToString();
|
|
}
|
|
}
|
|
else if (FOptionalProperty* OptionalProperty = CastField<FOptionalProperty>(Property))
|
|
{
|
|
void* OptionalPtr = OptionalProperty->ContainerPtrToValuePtr<void>(Container);
|
|
if (OptionalPtr && OptionalProperty->IsSet(OptionalPtr))
|
|
{
|
|
FProperty* InnerProperty = OptionalProperty->GetValueProperty();
|
|
if (InnerProperty)
|
|
{
|
|
ValueStr = GetPropertyValueAsString(InnerProperty, OptionalPtr);
|
|
}
|
|
}
|
|
}
|
|
else if (FStructProperty* StructProperty = CastField<FStructProperty>(Property))
|
|
{
|
|
if (StructProperty->Struct->GetFName() == NAME_Vector)
|
|
{
|
|
FVector* Vector = StructProperty->ContainerPtrToValuePtr<FVector>(Container);
|
|
ValueStr = FString::Printf(TEXT("%f %f %f"), Vector->X, Vector->Y, Vector->Z);
|
|
}
|
|
else if (StructProperty->Struct->GetFName() == NAME_Vector2D)
|
|
{
|
|
FVector2D* Vector = StructProperty->ContainerPtrToValuePtr<FVector2D>(Container);
|
|
ValueStr = FString::Printf(TEXT("%f %f"), Vector->X, Vector->Y);
|
|
}
|
|
else if (StructProperty->Struct->GetFName() == NAME_IntPoint)
|
|
{
|
|
FIntPoint* IntPoint = StructProperty->ContainerPtrToValuePtr<FIntPoint>(Container);
|
|
ValueStr = FString::Printf(TEXT("%d %d"), IntPoint->X, IntPoint->Y);
|
|
}
|
|
else
|
|
{
|
|
ValueStr = GetStructToString<FMujocoFriction>(StructProperty, Container);
|
|
if (ValueStr.IsEmpty())
|
|
ValueStr = GetStructToString<FMujocoSolImp>(StructProperty, Container);
|
|
if (ValueStr.IsEmpty())
|
|
ValueStr = GetStructToString<FMujocoSolRef>(StructProperty, Container);
|
|
if (ValueStr.IsEmpty())
|
|
ValueStr = GetStructToString<FMujocoRelPose>(StructProperty, Container);
|
|
if (ValueStr.IsEmpty())
|
|
ValueStr = GetStructToString<FMujocoPolyCoef>(StructProperty, Container);
|
|
if (ValueStr.IsEmpty())
|
|
ValueStr = GetStructToString<FMujocoGear>(StructProperty, Container);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
UE_LOG(LogMujoco, Warning, TEXT("Unsupported property type %s"), *Property->GetClass()->GetName());
|
|
}
|
|
return ValueStr;
|
|
}
|
|
|
|
template <> FString GetAttributeMappingForProperty<UMujocoBodyComponent>(FProperty* Prop)
|
|
{
|
|
// clang-format off
|
|
constexpr std::array Mapping{
|
|
std::pair{ TEXT("Mocap"), TEXT("mocap") },
|
|
std::pair{ TEXT("GravComp"), TEXT("gravcomp") }
|
|
};
|
|
// clang-format on
|
|
|
|
FString PropName = Prop->GetNameCPP();
|
|
for (const auto& Pair : Mapping)
|
|
{
|
|
if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
|
|
{
|
|
return FString(Pair.second);
|
|
}
|
|
}
|
|
return FString();
|
|
}
|
|
|
|
template <typename T> FString GetPropertyAttributeName(FProperty* Prop)
|
|
{
|
|
return GetAttributeMappingForProperty<T>(Prop);
|
|
}
|
|
|
|
template <typename T> void ApplyStructAttributes(tinyxml2::XMLElement* XmlElem, const T& StructData)
|
|
{
|
|
for (TFieldIterator<FProperty> It(T::StaticStruct()); It; ++It)
|
|
{
|
|
FProperty* Prop = *It;
|
|
const FString AttributeName = GetPropertyAttributeName<T>(Prop);
|
|
if (AttributeName.IsEmpty())
|
|
{
|
|
continue;
|
|
}
|
|
const FString Value = GetPropertyValueAsString(Prop, reinterpret_cast<void*>(const_cast<T*>(&StructData)));
|
|
if (Value.IsEmpty())
|
|
{
|
|
continue;
|
|
}
|
|
XmlElem->SetAttribute(TCHAR_TO_ANSI(*AttributeName), TCHAR_TO_ANSI(*Value));
|
|
}
|
|
}
|
|
|
|
bool FMujocoXmlGenerator::VisitGeomComponent(USCS_Node* SCSNode, UMujocoGeomComponent* GeomComponent, tinyxml2::XMLElement* Element)
|
|
{
|
|
FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : GeomComponent->GetName();
|
|
|
|
tinyxml2::XMLElement* GeomElement = Element->GetDocument()->NewElement("geom");
|
|
const FMujocoGeom& GeomStruct = GeomComponent->Geom;
|
|
GeomElement->SetAttribute("name", TCHAR_TO_ANSI(*Name));
|
|
GeomElement->SetAttribute("type", "mesh");
|
|
FTransform BodyTransform = GeomComponent->GetRelativeTransform();
|
|
FVector Position = BodyTransform.GetLocation() * 0.01f;
|
|
FQuat Rotation = BodyTransform.Rotator().Quaternion();
|
|
FVector Scale = BodyTransform.GetScale3D();
|
|
if (!Position.IsZero())
|
|
{
|
|
GeomElement->SetAttribute("pos", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f"), Position.X, -Position.Y, Position.Z)));
|
|
}
|
|
if (!Rotation.IsIdentity())
|
|
{
|
|
GeomElement->SetAttribute("quat", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f %f"), -Rotation.W, Rotation.X, -Rotation.Y, Rotation.Z)));
|
|
}
|
|
ApplyStructAttributes(GeomElement, GeomStruct);
|
|
if (UStaticMesh* StaticMesh = GeomComponent->GetStaticMesh())
|
|
{
|
|
tinyxml2::XMLElement* Root = Element->GetDocument()->FirstChildElement("mujoco");
|
|
ensureMsgf(Root, TEXT("No mujoco root element found in xml document"));
|
|
tinyxml2::XMLElement* AssetRoot = Root->FirstChildElement("asset");
|
|
ensureMsgf(AssetRoot, TEXT("No asset element found in mujoco root element"));
|
|
tinyxml2::XMLElement* MeshElement = Element->GetDocument()->NewElement("mesh");
|
|
FString MeshPath = GeomComponent->GetFName().ToString();
|
|
ObjectMap.FindOrAdd(MeshPath, GeomComponent);
|
|
FString MeshName = FString::Printf(TEXT("%s_%s_%d"), *Name, *StaticMesh->GetName(), StaticMesh->GetUniqueID());
|
|
MeshElement->SetAttribute("file", TCHAR_TO_ANSI(*MeshPath));
|
|
MeshElement->SetAttribute("name", TCHAR_TO_ANSI(*MeshName));
|
|
if (Scale.X != 1.0f || Scale.Y != 1.0f || Scale.Z != 1.0f)
|
|
{
|
|
MeshElement->SetAttribute("scale", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f"), Scale.X, Scale.Y, Scale.Z)));
|
|
}
|
|
AssetRoot->InsertEndChild(MeshElement);
|
|
GeomElement->SetAttribute("mesh", TCHAR_TO_ANSI(*MeshName));
|
|
}
|
|
Element->InsertEndChild(GeomElement);
|
|
return true;
|
|
}
|
|
|
|
bool FMujocoXmlGenerator::VisitJointComponent(USCS_Node* SCSNode, UMujocoJointComponent* JointComponent, tinyxml2::XMLElement* Element)
|
|
{
|
|
FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : JointComponent->GetName();
|
|
|
|
tinyxml2::XMLElement* JointElement = Element->GetDocument()->NewElement("joint");
|
|
const FMujocoJoint& JointStruct = JointComponent->Joint;
|
|
JointElement->SetAttribute("name", TCHAR_TO_ANSI(*Name));
|
|
ApplyStructAttributes(JointElement, JointStruct);
|
|
Element->InsertEndChild(JointElement);
|
|
return true;
|
|
}
|
|
|
|
bool FMujocoXmlGenerator::VisitSiteComponent(USCS_Node* SCSNode, UMujocoSiteComponent* SiteComponent, tinyxml2::XMLElement* Element)
|
|
{
|
|
FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : SiteComponent->GetName();
|
|
|
|
tinyxml2::XMLElement* SiteElement = Element->GetDocument()->NewElement("site");
|
|
const FMujocoSite& SiteStruct = SiteComponent->Site;
|
|
SiteElement->SetAttribute("name", TCHAR_TO_ANSI(*Name));
|
|
FTransform SiteTransform = SiteComponent->GetRelativeTransform();
|
|
FVector Position = SiteTransform.GetLocation() * 0.01f;
|
|
FQuat Rotation = SiteTransform.Rotator().Quaternion();
|
|
if (!Position.IsZero())
|
|
{
|
|
SiteElement->SetAttribute("pos", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f"), Position.X, -Position.Y, Position.Z)));
|
|
}
|
|
if (!Rotation.IsIdentity())
|
|
{
|
|
SiteElement->SetAttribute("quat", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f %f"), -Rotation.W, Rotation.X, -Rotation.Y, Rotation.Z)));
|
|
}
|
|
ApplyStructAttributes(SiteElement, SiteStruct);
|
|
Element->InsertEndChild(SiteElement);
|
|
return true;
|
|
}
|
|
|
|
bool FMujocoXmlGenerator::VisitActuatorComponent(USCS_Node* SCSNode, UMujocoActuatorComponent* ActuatorComponent, tinyxml2::XMLElement* Element)
|
|
{
|
|
FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : ActuatorComponent->GetName();
|
|
|
|
tinyxml2::XMLElement* ActuatorElement = nullptr;
|
|
const FMujocoActuatorV2& ActuatorStruct = ActuatorComponent->Actuator;
|
|
if (FEnumProperty* TypeProperty = CastField<FEnumProperty>(FMujocoActuatorV2::StaticStruct()->FindPropertyByName("Type")))
|
|
{
|
|
int64 EnumValue = TypeProperty->GetUnderlyingProperty()->GetSignedIntPropertyValue(TypeProperty->ContainerPtrToValuePtr<void>(&ActuatorStruct));
|
|
auto DisplayText = TypeProperty->GetEnum()->GetNameStringByValue(EnumValue);
|
|
auto ValueStr = DisplayText.ToLower();
|
|
ActuatorElement = Element->GetDocument()->NewElement(TCHAR_TO_ANSI(*ValueStr));
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
ActuatorElement->SetAttribute("name", TCHAR_TO_ANSI(*Name));
|
|
ApplyStructAttributes(ActuatorElement, ActuatorStruct);
|
|
tinyxml2::XMLElement* ActuatorRoot = Element->FirstChildElement("actuator");
|
|
ensureMsgf(ActuatorRoot, TEXT("No actuator element found in mujoco root element"));
|
|
ActuatorRoot->InsertEndChild(ActuatorElement);
|
|
return true;
|
|
}
|
|
|
|
bool FMujocoXmlGenerator::VisitEqualityComponent(USCS_Node* SCSNode, UMujocoEqualityComponent* EqualityComponent, tinyxml2::XMLElement* Element)
|
|
{
|
|
FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : EqualityComponent->GetName();
|
|
|
|
tinyxml2::XMLElement* EqualityElement = nullptr;
|
|
const FMujocoEquality& EqualityStruct = EqualityComponent->Equality;
|
|
if (FEnumProperty* TypeProperty = CastField<FEnumProperty>(FMujocoEquality::StaticStruct()->FindPropertyByName("Type")))
|
|
{
|
|
int64 EnumValue = TypeProperty->GetUnderlyingProperty()->GetSignedIntPropertyValue(TypeProperty->ContainerPtrToValuePtr<void>(&EqualityStruct));
|
|
auto DisplayText = TypeProperty->GetEnum()->GetNameStringByValue(EnumValue);
|
|
auto ValueStr = DisplayText.ToLower();
|
|
EqualityElement = Element->GetDocument()->NewElement(TCHAR_TO_ANSI(*ValueStr));
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
ApplyStructAttributes(EqualityElement, EqualityStruct);
|
|
tinyxml2::XMLElement* EqualityRoot = Element->FirstChildElement("equality");
|
|
ensureMsgf(EqualityRoot, TEXT("No equality element found in mujoco root element"));
|
|
EqualityRoot->InsertEndChild(EqualityElement);
|
|
return true;
|
|
}
|
|
|
|
bool FMujocoXmlGenerator::VisitTendonComponent(USCS_Node* SCSNode, UMujocoTendonComponent* TendonComponent, tinyxml2::XMLElement* Element)
|
|
{
|
|
FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : TendonComponent->GetName();
|
|
|
|
tinyxml2::XMLElement* TendonElement = nullptr;
|
|
const FMujocoTendon& TendonStruct = TendonComponent->Tendon;
|
|
if (FEnumProperty* TypeProperty = CastField<FEnumProperty>(FMujocoTendon::StaticStruct()->FindPropertyByName("Type")))
|
|
{
|
|
int64 EnumValue = TypeProperty->GetUnderlyingProperty()->GetSignedIntPropertyValue(TypeProperty->ContainerPtrToValuePtr<void>(&TendonStruct));
|
|
auto DisplayText = TypeProperty->GetEnum()->GetNameStringByValue(EnumValue);
|
|
auto ValueStr = DisplayText.ToLower();
|
|
TendonElement = Element->GetDocument()->NewElement(TCHAR_TO_ANSI(*ValueStr));
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
ApplyStructAttributes(TendonElement, TendonStruct);
|
|
for (const auto& SpatialSite : TendonComponent->Tendon.SpatialSite)
|
|
{
|
|
tinyxml2::XMLElement* SpatialElement = TendonElement->GetDocument()->NewElement("site");
|
|
ApplyStructAttributes(SpatialElement, SpatialSite);
|
|
TendonElement->InsertEndChild(SpatialElement);
|
|
}
|
|
for (const auto& SpatialGeom : TendonComponent->Tendon.SpatialGeom)
|
|
{
|
|
tinyxml2::XMLElement* SpatialElement = TendonElement->GetDocument()->NewElement("geom");
|
|
ApplyStructAttributes(SpatialElement, SpatialGeom);
|
|
TendonElement->InsertEndChild(SpatialElement);
|
|
}
|
|
for (const auto& Pulley : TendonComponent->Tendon.Pulley)
|
|
{
|
|
tinyxml2::XMLElement* PulleyElement = TendonElement->GetDocument()->NewElement("pulley");
|
|
ApplyStructAttributes(PulleyElement, Pulley);
|
|
TendonElement->InsertEndChild(PulleyElement);
|
|
}
|
|
for (const auto& FixedJoint : TendonComponent->Tendon.FixedJoint)
|
|
{
|
|
tinyxml2::XMLElement* FixedElement = TendonElement->GetDocument()->NewElement("joint");
|
|
ApplyStructAttributes(FixedElement, FixedJoint);
|
|
TendonElement->InsertEndChild(FixedElement);
|
|
}
|
|
tinyxml2::XMLElement* TendonRoot = Element->FirstChildElement("tendon");
|
|
ensureMsgf(TendonRoot, TEXT("No tendon element found in mujoco root element"));
|
|
TendonRoot->InsertEndChild(TendonElement);
|
|
return true;
|
|
}
|
|
|
|
bool FMujocoXmlGenerator::VisitCameraComponent(USCS_Node* SCSNode, UCameraComponent* CameraComponent, tinyxml2::XMLElement* Element, USpringArmComponent* SpringArmComponent)
|
|
{
|
|
FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : CameraComponent->GetName();
|
|
|
|
if (SpringArmComponent)
|
|
{
|
|
FTransform SpringArmTransform = SpringArmComponent->GetRelativeTransform();
|
|
float SpringArmLength = SpringArmComponent->TargetArmLength * 0.01f;
|
|
FVector Position = SpringArmTransform.GetLocation() * 0.1f - SpringArmTransform.GetRotation().GetForwardVector() * SpringArmLength;
|
|
FQuat Rotation = SpringArmTransform.GetRotation().Rotator().Quaternion();
|
|
tinyxml2::XMLElement* FrameElement = Element->GetDocument()->NewElement("frame");
|
|
tinyxml2::XMLElement* CameraElement = Element->GetDocument()->NewElement("camera");
|
|
CameraElement->SetAttribute("name", TCHAR_TO_ANSI(*Name));
|
|
if (!Position.IsZero())
|
|
{
|
|
FrameElement->SetAttribute("pos", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f"), Position.X, -Position.Y, Position.Z)));
|
|
}
|
|
if (!Rotation.IsIdentity())
|
|
{
|
|
FrameElement->SetAttribute("quat", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f %f"), -Rotation.W, Rotation.X, -Rotation.Y, Rotation.Z)));
|
|
}
|
|
auto ElementName = Element->Name();
|
|
if (std::strcmp(ElementName, "body") == 0)
|
|
{
|
|
auto BodyName = Element->Attribute("name");
|
|
CameraElement->SetAttribute("mode", "targetbody");
|
|
CameraElement->SetAttribute("target", BodyName);
|
|
}
|
|
else
|
|
{
|
|
CameraElement->SetAttribute("mode", "track");
|
|
}
|
|
FrameElement->InsertEndChild(CameraElement);
|
|
Element->InsertEndChild(FrameElement);
|
|
}
|
|
else
|
|
{
|
|
tinyxml2::XMLElement* FrameElement = Element->GetDocument()->NewElement("frame");
|
|
tinyxml2::XMLElement* CameraElement = Element->GetDocument()->NewElement("camera");
|
|
CameraElement->SetAttribute("name", TCHAR_TO_ANSI(*Name));
|
|
FTransform CameraTransform = CameraComponent->GetRelativeTransform();
|
|
FVector Position = CameraTransform.GetLocation() * 0.01f;
|
|
const FQuat CamExtra = FQuat(FRotator(0, -90, -90));
|
|
CameraElement->SetAttribute("quat", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f %f"), CamExtra.W, CamExtra.X, CamExtra.Y, CamExtra.Z)));
|
|
FQuat Rotation = CameraTransform.Rotator().Quaternion();
|
|
if (!Position.IsZero())
|
|
{
|
|
FrameElement->SetAttribute("pos", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f"), Position.X, -Position.Y, Position.Z)));
|
|
}
|
|
if (!Rotation.IsIdentity())
|
|
{
|
|
FrameElement->SetAttribute("quat", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f %f"), -Rotation.W, Rotation.X, -Rotation.Y, Rotation.Z)));
|
|
}
|
|
FrameElement->InsertEndChild(CameraElement);
|
|
Element->InsertEndChild(FrameElement);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool FMujocoXmlGenerator::VisitSpringArm(USCS_Node* SCSNode, USpringArmComponent* SpringArmComponent, tinyxml2::XMLElement* Element)
|
|
{
|
|
FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : SpringArmComponent->GetName();
|
|
|
|
if (!SCSNode)
|
|
{
|
|
TArray<USceneComponent*> CameraComponents;
|
|
SpringArmComponent->GetChildrenComponents(true, CameraComponents);
|
|
for (USceneComponent* ChildComponent : CameraComponents)
|
|
{
|
|
if (UCameraComponent* CameraComponent = Cast<UCameraComponent>(ChildComponent))
|
|
{
|
|
if (!VisitCameraComponent(SCSNode, CameraComponent, Element, SpringArmComponent))
|
|
{
|
|
return false;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for (USCS_Node* ChildNode : SCSNode->GetChildNodes())
|
|
{
|
|
if (UCameraComponent* CameraComponent = Cast<UCameraComponent>(ChildNode->GetActualComponentTemplate(SpringArmComponent->GetTypedOuter<UBlueprintGeneratedClass>())))
|
|
{
|
|
if (!VisitCameraComponent(ChildNode, CameraComponent, Element, SpringArmComponent))
|
|
{
|
|
return false;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool FMujocoXmlGenerator::VisitBodyComponent(USCS_Node* SCSNode, UMujocoBodyComponent* BodyComponent, tinyxml2::XMLElement* Element)
|
|
{
|
|
FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : BodyComponent->GetName();
|
|
|
|
tinyxml2::XMLElement* BodyElement = Element->GetDocument()->NewElement("body");
|
|
BodyElement->SetAttribute("name", TCHAR_TO_ANSI(*Name));
|
|
FTransform BodyTransform = BodyComponent->GetRelativeTransform();
|
|
FVector Position = BodyTransform.GetLocation() * 0.01f;
|
|
FQuat Rotation = BodyTransform.Rotator().Quaternion();
|
|
if (!Position.IsZero())
|
|
{
|
|
BodyElement->SetAttribute("pos", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f"), Position.X, -Position.Y, Position.Z)));
|
|
}
|
|
if (!Rotation.IsIdentity())
|
|
{
|
|
BodyElement->SetAttribute("quat", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f %f"), -Rotation.W, Rotation.X, -Rotation.Y, Rotation.Z)));
|
|
}
|
|
|
|
for (TFieldIterator<FProperty> PropIt(BodyComponent->GetClass()); PropIt; ++PropIt)
|
|
{
|
|
FProperty* Prop = *PropIt;
|
|
const FString AttributeName = GetPropertyAttributeName<UMujocoBodyComponent>(Prop);
|
|
if (AttributeName.IsEmpty())
|
|
{
|
|
continue;
|
|
}
|
|
const FString Value = GetPropertyValueAsString(Prop, BodyComponent);
|
|
if (Value.IsEmpty())
|
|
{
|
|
continue;
|
|
}
|
|
BodyElement->SetAttribute(TCHAR_TO_ANSI(*AttributeName), TCHAR_TO_ANSI(*Value));
|
|
}
|
|
|
|
if (BodyComponent->Inertial.IsSet())
|
|
{
|
|
tinyxml2::XMLElement* InertialElement = BodyElement->GetDocument()->NewElement("inertial");
|
|
const FMujocoInertial& InertialStruct = BodyComponent->Inertial.GetValue();
|
|
ApplyStructAttributes(InertialElement, InertialStruct);
|
|
BodyElement->InsertEndChild(InertialElement);
|
|
}
|
|
if (SCSNode)
|
|
{
|
|
for (USCS_Node* ChildNode : SCSNode->GetChildNodes())
|
|
{
|
|
if (UMujocoBodyComponent* ChildBodyComponent = Cast<UMujocoBodyComponent>(ChildNode->GetActualComponentTemplate(BodyComponent->GetTypedOuter<UBlueprintGeneratedClass>())))
|
|
{
|
|
if (!VisitBodyComponent(ChildNode, ChildBodyComponent, BodyElement))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (UMujocoGeomComponent* GeomComponent = Cast<UMujocoGeomComponent>(ChildNode->GetActualComponentTemplate(BodyComponent->GetTypedOuter<UBlueprintGeneratedClass>())))
|
|
{
|
|
if (!VisitGeomComponent(ChildNode, GeomComponent, BodyElement))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (UMujocoJointComponent* JointComponent = Cast<UMujocoJointComponent>(ChildNode->GetActualComponentTemplate(BodyComponent->GetTypedOuter<UBlueprintGeneratedClass>())))
|
|
{
|
|
if (!VisitJointComponent(ChildNode, JointComponent, BodyElement))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (UMujocoSiteComponent* SiteComponent = Cast<UMujocoSiteComponent>(ChildNode->GetActualComponentTemplate(BodyComponent->GetTypedOuter<UBlueprintGeneratedClass>())))
|
|
{
|
|
if (!VisitSiteComponent(ChildNode, SiteComponent, BodyElement))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (UCameraComponent* CameraComponent = Cast<UCameraComponent>(ChildNode->GetActualComponentTemplate(BodyComponent->GetTypedOuter<UBlueprintGeneratedClass>())))
|
|
{
|
|
if (!VisitCameraComponent(ChildNode, CameraComponent, BodyElement))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (USpringArmComponent* SpringArmComponent = Cast<USpringArmComponent>(ChildNode->GetActualComponentTemplate(BodyComponent->GetTypedOuter<UBlueprintGeneratedClass>())))
|
|
{
|
|
if (!VisitSpringArm(ChildNode, SpringArmComponent, BodyElement))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
TArray<USceneComponent*> ChildComponents;
|
|
BodyComponent->GetChildrenComponents(false, ChildComponents);
|
|
|
|
TSet<USceneComponent*> UniqueChildrens;
|
|
for (USceneComponent* ChildComponent : ChildComponents)
|
|
{
|
|
UniqueChildrens.Add(ChildComponent);
|
|
}
|
|
|
|
for (USceneComponent* ChildComponent : UniqueChildrens)
|
|
{
|
|
if (UMujocoBodyComponent* ChildBodyComponent = Cast<UMujocoBodyComponent>(ChildComponent))
|
|
{
|
|
if (!VisitBodyComponent(nullptr, ChildBodyComponent, BodyElement))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (UMujocoGeomComponent* GeomComponent = Cast<UMujocoGeomComponent>(ChildComponent))
|
|
{
|
|
if (!VisitGeomComponent(nullptr, GeomComponent, BodyElement))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (UMujocoJointComponent* JointComponent = Cast<UMujocoJointComponent>(ChildComponent))
|
|
{
|
|
if (!VisitJointComponent(nullptr, JointComponent, BodyElement))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (UMujocoSiteComponent* SiteComponent = Cast<UMujocoSiteComponent>(ChildComponent))
|
|
{
|
|
if (!VisitSiteComponent(nullptr, SiteComponent, BodyElement))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (UCameraComponent* CameraComponent = Cast<UCameraComponent>(ChildComponent))
|
|
{
|
|
if (!VisitCameraComponent(nullptr, CameraComponent, BodyElement))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (USpringArmComponent* SpringArmComponent = Cast<USpringArmComponent>(ChildComponent))
|
|
{
|
|
if (!VisitSpringArm(nullptr, SpringArmComponent, BodyElement))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if (std::strcmp(Element->Name(), "mujoco") == 0)
|
|
{
|
|
tinyxml2::XMLElement* WorldBodyElement = Element->FirstChildElement("worldbody");
|
|
ensureMsgf(WorldBodyElement, TEXT("No worldbody element found in mujoco root element"));
|
|
WorldBodyElement->InsertEndChild(BodyElement);
|
|
}
|
|
else if (std::strcmp(Element->Name(), "body") == 0)
|
|
{
|
|
Element->InsertEndChild(BodyElement);
|
|
}
|
|
else
|
|
{
|
|
ensureMsgf(false, TEXT("Unexpected element type"));
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
template <typename T> TMap<FString, FString> ProcessProperties(void* StructPtr)
|
|
{
|
|
TMap<FString, FString> OptionsMap;
|
|
UStruct* Struct = T::StaticStruct();
|
|
for (TFieldIterator<FProperty> PropIt(Struct); PropIt; ++PropIt)
|
|
{
|
|
FProperty* Property = *PropIt;
|
|
const FString AttributeName = GetAttributeMappingForProperty<T>(Property);
|
|
if (AttributeName.IsEmpty())
|
|
{
|
|
continue;
|
|
}
|
|
FName OverridePropName(*FString::Printf(TEXT("bOverride%s"), *Property->GetNameCPP()));
|
|
if (FProperty* OverrideProperty = Struct->FindPropertyByName(OverridePropName))
|
|
{
|
|
void* OverridePtr = OverrideProperty->ContainerPtrToValuePtr<void>(StructPtr);
|
|
bool bOverride = false;
|
|
OverrideProperty->GetValue_InContainer(StructPtr, &bOverride);
|
|
if (!bOverride)
|
|
{
|
|
continue;
|
|
}
|
|
}
|
|
const FString ValueStr = GetPropertyValueAsString(Property, StructPtr);
|
|
OptionsMap.Add(AttributeName, ValueStr);
|
|
}
|
|
return OptionsMap;
|
|
}
|
|
|
|
bool FMujocoXmlGenerator::ParseExportOptions(UMujocoExportOptions* Data, tinyxml2::XMLElement* Element)
|
|
{
|
|
if (!Data || !Element)
|
|
{
|
|
return false;
|
|
}
|
|
Element->SetAttribute("model", TCHAR_TO_ANSI(*Data->ModelName));
|
|
|
|
TMap<FString, FString> CompilerOptions = ProcessProperties<FMujocoCompileOptions>(&Data->Compiler);
|
|
TMap<FString, FString> MujocoOptions = ProcessProperties<FMujocoOptions>(&Data->Options);
|
|
TMap<FString, FString> SimulationFlags = ProcessProperties<FMujocoSimulationFlags>(&Data->SimulationFlags);
|
|
|
|
if (CompilerOptions.Num() > 0)
|
|
{
|
|
tinyxml2::XMLElement* CompilerElement = Element->InsertNewChildElement("compiler");
|
|
for (const auto& [Key, Value] : CompilerOptions)
|
|
{
|
|
CompilerElement->SetAttribute(TCHAR_TO_ANSI(*Key), TCHAR_TO_ANSI(*Value));
|
|
}
|
|
}
|
|
if (MujocoOptions.Num() > 0 || SimulationFlags.Num() > 0)
|
|
{
|
|
tinyxml2::XMLElement* MujocoElement = Element->InsertNewChildElement("option");
|
|
for (const auto& [Key, Value] : MujocoOptions)
|
|
{
|
|
MujocoElement->SetAttribute(TCHAR_TO_ANSI(*Key), TCHAR_TO_ANSI(*Value));
|
|
}
|
|
if (SimulationFlags.Num() > 0)
|
|
{
|
|
tinyxml2::XMLElement* FlagsElement = MujocoElement->InsertNewChildElement("flag");
|
|
for (const auto& [Key, Value] : SimulationFlags)
|
|
{
|
|
FlagsElement->SetAttribute(TCHAR_TO_ANSI(*Key), TCHAR_TO_ANSI(*Value));
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool FMujocoXmlGenerator::ParseBlueprintAsset(UBlueprint* BlueprintAsset, tinyxml2::XMLElement* Root)
|
|
{
|
|
if (!BlueprintAsset || !BlueprintAsset->GeneratedClass)
|
|
{
|
|
return false;
|
|
}
|
|
AActor* DefaultActor = Cast<AActor>(BlueprintAsset->GeneratedClass->GetDefaultObject());
|
|
if (!DefaultActor)
|
|
{
|
|
return false;
|
|
}
|
|
if (!BlueprintAsset->SimpleConstructionScript)
|
|
{
|
|
return false;
|
|
}
|
|
if (UBlueprintGeneratedClass* BPGeneratedClass = Cast<UBlueprintGeneratedClass>(BlueprintAsset->GeneratedClass))
|
|
{
|
|
for (USCS_Node* SCSNode : BlueprintAsset->SimpleConstructionScript->GetRootNodes())
|
|
{
|
|
if (!SCSNode)
|
|
{
|
|
continue;
|
|
}
|
|
if (UMujocoBodyComponent* BodyComp = Cast<UMujocoBodyComponent>(SCSNode->GetActualComponentTemplate(BPGeneratedClass)))
|
|
{
|
|
if (!VisitBodyComponent(SCSNode, BodyComp, Root))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (UMujocoActuatorComponent* ActuatorComp = Cast<UMujocoActuatorComponent>(SCSNode->GetActualComponentTemplate(BPGeneratedClass)))
|
|
{
|
|
if (!VisitActuatorComponent(SCSNode, ActuatorComp, Root))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (UMujocoEqualityComponent* EqualityComp = Cast<UMujocoEqualityComponent>(SCSNode->GetActualComponentTemplate(BPGeneratedClass)))
|
|
{
|
|
if (!VisitEqualityComponent(SCSNode, EqualityComp, Root))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (UMujocoTendonComponent* TendonComp = Cast<UMujocoTendonComponent>(SCSNode->GetActualComponentTemplate(BPGeneratedClass)))
|
|
{
|
|
if (!VisitTendonComponent(SCSNode, TendonComp, Root))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool FMujocoXmlGenerator::ParseActorAsset(AActor* Actor, tinyxml2::XMLElement* Root)
|
|
{
|
|
if (!Actor)
|
|
{
|
|
return false;
|
|
}
|
|
for (UActorComponent* Component : Actor->GetComponents())
|
|
{
|
|
if (UMujocoBodyComponent* BodyComp = Cast<UMujocoBodyComponent>(Component))
|
|
{
|
|
if (BodyComp->GetAttachParent())
|
|
{
|
|
continue;
|
|
}
|
|
if (!VisitBodyComponent(nullptr, BodyComp, Root))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (UMujocoActuatorComponent* ActuatorComp = Cast<UMujocoActuatorComponent>(Component))
|
|
{
|
|
if (!VisitActuatorComponent(nullptr, ActuatorComp, Root))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (UMujocoEqualityComponent* EqualityComp = Cast<UMujocoEqualityComponent>(Component))
|
|
{
|
|
if (!VisitEqualityComponent(nullptr, EqualityComp, Root))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else if (UMujocoTendonComponent* TendonComp = Cast<UMujocoTendonComponent>(Component))
|
|
{
|
|
if (!VisitTendonComponent(nullptr, TendonComp, Root))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
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\""));
|
|
tinyxml2::XMLElement* Root = Doc->NewElement("mujoco");
|
|
tinyxml2::XMLElement* AssetRoot = Doc->NewElement("asset");
|
|
tinyxml2::XMLElement* WorldBodyRoot = Doc->NewElement("worldbody");
|
|
ParseExportOptions(ExportOptions, Root);
|
|
Root->InsertEndChild(AssetRoot);
|
|
Root->InsertEndChild(WorldBodyRoot);
|
|
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");
|
|
TextureElement->SetAttribute("type", "skybox");
|
|
TextureElement->SetAttribute("builtin", "gradient");
|
|
TextureElement->SetAttribute("rgb1", "0.3 0.5 0.7");
|
|
TextureElement->SetAttribute("rgb2", "0 0 0");
|
|
TextureElement->SetAttribute("width", "512");
|
|
TextureElement->SetAttribute("height", "3072");
|
|
AssetRoot->InsertEndChild(TextureElement);
|
|
tinyxml2::XMLElement* LightElement = WorldBodyRoot->GetDocument()->NewElement("light");
|
|
LightElement->SetAttribute("pos", "0 0 1.5");
|
|
LightElement->SetAttribute("dir", "0 0 -1");
|
|
LightElement->SetAttribute("directional", "true");
|
|
WorldBodyRoot->InsertEndChild(LightElement);
|
|
}
|
|
if (ExportOptions->bAddGroundPlane)
|
|
{
|
|
tinyxml2::XMLElement* TextureElement = AssetRoot->GetDocument()->NewElement("texture");
|
|
TextureElement->SetAttribute("type", "2d");
|
|
TextureElement->SetAttribute("name", "groundplane");
|
|
TextureElement->SetAttribute("builtin", "checker");
|
|
TextureElement->SetAttribute("mark", "edge");
|
|
TextureElement->SetAttribute("rgb1", "0.2 0.3 0.4");
|
|
TextureElement->SetAttribute("rgb2", "0.1 0.2 0.3");
|
|
TextureElement->SetAttribute("markrgb", "0.8 0.8 0.8");
|
|
TextureElement->SetAttribute("width", "300");
|
|
TextureElement->SetAttribute("height", "300");
|
|
AssetRoot->InsertEndChild(TextureElement);
|
|
tinyxml2::XMLElement* MaterialElement = AssetRoot->GetDocument()->NewElement("material");
|
|
MaterialElement->SetAttribute("name", "groundplane");
|
|
MaterialElement->SetAttribute("texture", "groundplane");
|
|
MaterialElement->SetAttribute("texuniform", "true");
|
|
MaterialElement->SetAttribute("texrepeat", "100 100");
|
|
MaterialElement->SetAttribute("reflectance", "0.2");
|
|
AssetRoot->InsertEndChild(MaterialElement);
|
|
tinyxml2::XMLElement* GroundPlaneElement = WorldBodyRoot->GetDocument()->NewElement("geom");
|
|
GroundPlaneElement->SetAttribute("name", "floor");
|
|
GroundPlaneElement->SetAttribute("size", "0 0 0.05");
|
|
GroundPlaneElement->SetAttribute("type", "plane");
|
|
GroundPlaneElement->SetAttribute("material", "groundplane");
|
|
WorldBodyRoot->InsertEndChild(GroundPlaneElement);
|
|
}
|
|
Doc->InsertEndChild(Root);
|
|
for (UObject* Object : Objects)
|
|
{
|
|
if (UBlueprint* Blueprint = Cast<UBlueprint>(Object))
|
|
{
|
|
ParseBlueprintAsset(Blueprint, Root);
|
|
}
|
|
else if (AActor* Actor = Cast<AActor>(Object))
|
|
{
|
|
ParseActorAsset(Actor, Root);
|
|
}
|
|
}
|
|
TSet<FString> ExportedMeshes;
|
|
for (tinyxml2::XMLElement* MeshElement = AssetRoot->FirstChildElement("mesh"); MeshElement; MeshElement = MeshElement->NextSiblingElement("mesh"))
|
|
{
|
|
FString MeshPath = MeshElement->Attribute("file");
|
|
|
|
const auto& GeomIter = ObjectMap.Find(MeshPath);
|
|
if (!GeomIter->IsValid())
|
|
{
|
|
UE_LOG(LogTemp, Warning, TEXT("Failed to load geom %s not found"), *MeshPath);
|
|
continue;
|
|
}
|
|
|
|
auto* GeomObj = Cast<UMujocoGeomComponent>(GeomIter->Get());
|
|
if (!GeomObj)
|
|
{
|
|
UE_LOG(LogTemp, Warning, TEXT("Failed to load geom %s invalid ptr"), *MeshPath);
|
|
continue;
|
|
}
|
|
FString MeshName = FPaths::GetBaseFilename(MeshPath) + TEXT("_") + FString::FromInt(GeomObj->GetUniqueID());
|
|
FString MeshFilename = FPaths::Combine(FPaths::GetPath(ExportFilename), ExportOptions->Compiler.AssetDir, MeshName + TEXT(".obj"));
|
|
MeshFilename = FPaths::ConvertRelativePathToFull(MeshFilename);
|
|
if (!ExportedMeshes.Contains(MeshName))
|
|
{
|
|
ExportedMeshes.Add(MeshName);
|
|
if (UMujocoStaticMesh* MujocoMesh = Cast<UMujocoStaticMesh>(GeomObj->MujocoStaticMesh))
|
|
{
|
|
FFileHelper::SaveStringToFile(MujocoMesh->ObjFileData, *MeshFilename);
|
|
}
|
|
}
|
|
MeshElement->SetAttribute("file", TCHAR_TO_ANSI(*MeshFilename));
|
|
}
|
|
return MoveTemp(Doc);
|
|
} |