RREntityStructs.h

Go to the documentation of this file.

 
#pragma once
 
// UE
 
#include "ChaosVehicleWheel.h"
 
#include "CoreMinimal.h"
 
#include "Misc/Paths.h"
 
#include "Templates/SharedPointer.h"
 
 
 
// RapyutaSimulationPlugins
 
#include "Core/RRCoreUtils.h"
 
#include "Core/RRMathUtils.h"
 
#include "Core/RRTypeUtils.h"
 
 
 
#include "RREntityStructs.generated.h"
 
// (NOTE) To avoid cyclic inclusion, except for utils, please DO NOT include any other component header files here!
 
 
 
 
enum class ERRUEComponentType : uint8
 
{
 
    NONE = 0x00,
 
    DIFF_DRIVE = 0x01,
 
    WHEEL_DRIVE = 0x02,
 
    ARTICULATION_DRIVE = 0x04
 
};
 
ENUM_CLASS_FLAGS(ERRUEComponentType);
 
 
 
 
enum class ERREntityDescriptionType : uint8
 
{
 
    NONE,
 
    URDF,             
 
    SDF,              
 
    UE_DATA_TABLE,    
 
    SINGLE_CAD,       
 
    TOTAL
 
};
 
 
 
enum class ERRRobotStatus : uint8
 
{
 
    INVALID,             
 
    CREATING,            
 
    IDLE,                
 
    ORDER_PROCESSING,    
 
    MOVING,              
 
    ERROR,               
 
    TOTAL
 
};
 
 
 
 
enum class ERREntityGeometryType : uint8
 
{
 
    NONE = 0x00,
 
    VISUAL = 0x01,
 
    COLLISION = 0x02,
 
    INERTIA = 0x04
 
};
 
ENUM_CLASS_FLAGS(ERREntityGeometryType);
 
 
 
 
enum class ERRSensorType : uint8
 
{
 
    NONE,
 
    CAMERA,
 
    LIDAR,
 
    DEPTH,
 
    IMU,
 
    TOTAL
 
};
 
 
 
 
enum class ERRLidarSensorType : uint8
 
{
 
    NONE,
 
    TWO_D,
 
    THREE_D,
 
    TOTAL
 
};
 
 
 
 
enum class ERRRobotJointType : uint8
 
{
 
    NONE,
 
    FIXED,
 
    FLOATING,
 
    PRISMATIC,
 
    REVOLUTE,
 
    BALL,
 
    PLANAR,
 
    CONTINUOUS,
 
    TOTAL
 
};
 
 
 
 
enum class ERRRobotJointStatus : uint8
 
{
 
    INVALID,    
 
    IDLE,
 
    MOVING,
 
    TOTAL
 
};
 
 
 
 
enum class ERRJointAxisRotation : uint8
 
{
 
    NONE = 0x00,
 
    X = 0x01,
 
    Y = 0x02,
 
    Z = 0x04
 
};
 
 
 
 
enum class ERREntityMeshComponentType : uint8
 
{
 
    NONE,
 
    STATIC_MESH,
 
    SKELETAL_MESH,
 
    PROCEDURAL_MESH,
 
    DYNAMIC_MESH,
 
    TOTAL
 
};
 
 
 
// -----------------------------------------------------------------------------------------------
 
// [FRRRobotJointDynamicProperties] --
 
 
struct RAPYUTASIMULATIONPLUGINS_API FRRRobotJointDynamicProperties
 
{
 
    FRRRobotJointDynamicProperties()
 
    {
 
    }
 
    FRRRobotJointDynamicProperties(const FString& InJointName) : JointName(InJointName)
 
    {
 
    }
 
    FRRRobotJointDynamicProperties(float InStiffness, float InDamping, float InP, float InI, float InD)
 
    {
 
        SpringStiff = InStiffness;
 
        Damping = InDamping;
 
        P = InP;
 
        I = InI;
 
        D = InD;
 
    }
 
    FRRRobotJointDynamicProperties(const FString& InJointName, float InStiffness, float InDamping)
 
        : JointName(InJointName), SpringStiff(InStiffness), Damping(InDamping)
 
    {
 
    }
 
 
    FString JointName;
 
    // For Spring-Damper Control Mode --
 
 
 
              Category = "Spring-Damper",
 
              meta = (ClampMin = "0.0", ClampMax = "1000000000.0", UIMin = "0.0", UIMax = "1000000000.0"))
 
    float SpringStiff = 0.f;
 
 
              Category = "Spring-Damper",
 
              meta = (ClampMin = "0.0", ClampMax = "1000000000.0", UIMin = "0.0", UIMax = "1000000000.0"))
 
    float LimitSpringStiff = 0.f;
 
 
 
              Category = "Spring-Damper",
 
              meta = (ClampMin = "0.0", ClampMax = "500.0", UIMin = "0.0", UIMax = "500.0"))
 
    float Damping = 0.f;
 
 
              Category = "Spring-Damper",
 
              meta = (ClampMin = "0.0", ClampMax = "500.0", UIMin = "0.0", UIMax = "500.0"))
 
    float LimitDamping = 0.f;
 
 
              Category = "Spring-Damper",
 
              meta = (ClampMin = "0.0", ClampMax = "1000000000.0", UIMin = "0.0", UIMax = "1000000000.0"))
 
    float Friction = 0.f;
 
 
              Category = "Spring-Damper",
 
              meta = (ClampMin = "0.0", ClampMax = "1000000000.0", UIMin = "0.0", UIMax = "1000000000.0"))
 
    float SpringRef = 0.f;
 
 
              Category = "Spring-Damper",
 
              meta = (ClampMin = "0.0", ClampMax = "10000000.0", UIMin = "0.0", UIMax = "10000000.0"))
 
    float MaxForceLimit = 100000000.f;
 
 
 
 
 
    float MaxVelocity = 1000000.f;
 
 
 
 
 
    float KVelocity = 0.f;
 
 
 
 
 
    float P = 0.1f;
 
 
 
 
 
    float I = 0.5f;
 
 
 
 
 
    float D = 0.01f;
 
 
 
    void PrintSelf() const
 
    {
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- SpringStiff %f LimitSpringStiff %f"), SpringStiff, LimitSpringStiff);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Damping %f LimitDamping %f"), Damping, LimitDamping);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Friction %f SpringRef %f"), Friction, SpringRef);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- MaxForceLimit %f MaxVelocity %f"), MaxForceLimit, MaxVelocity);
 
    }
 
};
 
// [ROBOT JOINT] --
 
//
 
struct RAPYUTASIMULATIONPLUGINS_API FRRRobotJointProperty
 
{
 
public:
 
    FRRRobotJointProperty()
 
    {
 
    }
 
    FRRRobotJointProperty(FString InName) : Name(MoveTemp(InName))
 
    {
 
    }
 
 
 
 
    FString Name;
 
 
 
 
    ERRRobotJointType Type = ERRRobotJointType::NONE;
 
 
 
    FORCEINLINE static ERRRobotJointType GetERRRobotJointTypeValueFromString(const FString& InEnumStringValue)
 
    {
 
        return static_cast<ERRRobotJointType>(URRTypeUtils::GetEnumValueFromString(TEXT("ERRRobotJointType"), InEnumStringValue));
 
    }
 
 
 
    FORCEINLINE static FString GetJointTypeName(const ERRRobotJointType InJointType)
 
    {
 
        return URRTypeUtils::GetEnumValueAsString(TEXT("ERRRobotJointType"), InJointType);
 
    }
 
 
 
 
    ERRRobotJointStatus Status = ERRRobotJointStatus::INVALID;
 
 
 
 
 
    FVector Location = FVector::ZeroVector;
 
 
 
 
 
    FQuat Rotation = FQuat::Identity;
 
 
 
 
 
    bool bIsTransformRelative = true;
 
 
 
    FTransform GetTransform() const
 
    {
 
        return FTransform(Rotation, Location);
 
    }
 
 
    FString ParentLinkName;
 
 
    FString ChildLinkName;
 
 
    FString MimicJointName;
 
 
    float MimicMultiplier = 1.0f;
 
 
 
    float MimicOffset = 0.0f;
 
 
    FVector Axis = FVector::ZeroVector;
 
 
    FVector AxisInParentFrame = FVector::ZeroVector;
 
    const FVector& GetAxis(bool bIsLocal = false) const
 
    {
 
        return bIsLocal ? Axis : AxisInParentFrame;
 
    }
 
 
 
 
 
    float LowerLimit = 0.f;
 
 
 
 
 
    float UpperLimit = 0.f;
 
 
 
 
    FRRRobotJointDynamicProperties DynamicParams;
 
    bool operator==(const FRRRobotJointProperty& JointProp)
 
    {
 
        return Name.Equals(JointProp.Name);
 
    }
 
    bool operator==(const FString& String)
 
    {
 
        return Name.Equals(String);
 
    }
 
    bool IsValid() const
 
    {
 
        return (false == Name.IsEmpty()) && (ERRRobotJointType::NONE != Type);
 
    }
 
    ELinearConstraintMotion GetLinearConstraintMotion() const
 
    {
 
        switch (Type)
 
        {
 
            case ERRRobotJointType::NONE:
 
            case ERRRobotJointType::FIXED:
 
            case ERRRobotJointType::REVOLUTE:
 
            case ERRRobotJointType::CONTINUOUS:
 
            case ERRRobotJointType::BALL:
 
                return LCM_Locked;
 
 
 
            case ERRRobotJointType::PRISMATIC:
 
                return LCM_Limited;
 
 
 
            case ERRRobotJointType::PLANAR:
 
            case ERRRobotJointType::FLOATING:
 
                return LCM_Free;
 
 
 
            default:
 
                return LCM_Locked;
 
        }
 
    }
 
    EAngularConstraintMotion GetAngularConstraintMotion() const
 
    {
 
        switch (Type)
 
        {
 
            case ERRRobotJointType::NONE:
 
            case ERRRobotJointType::FIXED:
 
            case ERRRobotJointType::PRISMATIC:
 
                return ACM_Locked;
 
 
 
            case ERRRobotJointType::REVOLUTE:
 
            {
 
                return (UpperLimit > UE_TWO_PI) ? ACM_Free : ACM_Limited;
 
            }
 
 
 
            case ERRRobotJointType::FLOATING:
 
            case ERRRobotJointType::BALL:
 
            case ERRRobotJointType::PLANAR:
 
            case ERRRobotJointType::CONTINUOUS:
 
                return ACM_Free;
 
            default:
 
                return ACM_Locked;
 
        }
 
    }
 
 
 
    FVector GetJointAxis(bool bIsGlobal = false, const FQuat& InLinkQuat = FQuat::Identity) const
 
    {
 
        // https://github.com/ros-visualization/rviz/blob/noetic-devel/src/rviz/robot/robot_joint.cpp#L425
 
        return bIsGlobal ? InLinkQuat.RotateVector(Axis) : Axis;
 
    }
 
 
 
    bool IsTwisting(const FVector& InAxis) const
 
    {
 
        return FMath::IsNearlyEqual(FMath::Abs(InAxis.X), 1.f, 1.e-3f);
 
    }
 
 
 
    bool IsSwing1(const FVector& InAxis) const
 
    {
 
        return FMath::IsNearlyEqual(FMath::Abs(InAxis.Z), 1.f, 1.e-3f);
 
    }
 
 
 
    bool IsSwing2(const FVector& InAxis) const
 
    {
 
        return FMath::IsNearlyEqual(FMath::Abs(InAxis.Y), 1.f, 1.e-3f);
 
    }
 
    void GetTwistSwing(bool& bOutTwist, bool& bOutSwing1, bool& bOutSwing2) const
 
    {
 
        bOutTwist = IsTwisting(AxisInParentFrame);
 
        bOutSwing1 = IsSwing1(AxisInParentFrame);
 
        bOutSwing2 = IsSwing2(AxisInParentFrame);
 
    }
 
    void GetLinearXYZ(bool& bOutLinearX, bool& bOutLinearY, bool& bOutLinearZ) const
 
    {
 
        bOutLinearX = FMath::IsNearlyEqual(FMath::Abs(AxisInParentFrame.X), 1.f, 1.e-3f);
 
        bOutLinearY = FMath::IsNearlyEqual(FMath::Abs(AxisInParentFrame.Y), 1.f, 1.e-3f);
 
        bOutLinearZ = FMath::IsNearlyEqual(FMath::Abs(AxisInParentFrame.Z), 1.f, 1.e-3f);
 
    }
 
 
 
    bool IsValidJointPosition(float InJointPos) const
 
    {
 
        return (LowerLimit <= UpperLimit) ? (InJointPos >= LowerLimit) && (InJointPos <= UpperLimit) : false;
 
    }
 
    bool IsValidJointVelocity(float InJointVelocity) const
 
    {
 
        return (DynamicParams.MaxVelocity > 0.f) ? (FMath::Abs(InJointVelocity) <= DynamicParams.MaxVelocity)
 
                                                 : (InJointVelocity != 0.f);
 
    }
 
    void PrintSelf() const
 
    {
 
        UE_LOG_WITH_INFO(LogTemp,
 
                         Warning,
 
                         TEXT("Joint Name: %s - Type %s"),
 
                         *Name,
 
                         *URRTypeUtils::GetEnumValueAsString(TEXT("ERRRobotJointType"), Type));
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Location: %s"), *Location.ToString());
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Rotation: %s - %s"), *Rotation.ToString(), *FRotator(Rotation).ToString());
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- ParentLinkName: %s"), *ParentLinkName);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- ChildLinkName: %s"), *ChildLinkName);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Axis: %s"), *Axis.ToString());
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- MimicJointName: %s"), *MimicJointName);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- MimicMultiplier: %f"), MimicMultiplier);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- MimicOffset: %f"), MimicOffset);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- LowerLimit: %f"), LowerLimit);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- UpperLimit: %f"), UpperLimit);
 
        DynamicParams.PrintSelf();
 
    }
 
};
 
 
 
struct RAPYUTASIMULATIONPLUGINS_API FRRRobotJointValue
 
{
 
 
 
    double Position = 0;
 
 
 
    double LinearVel = 0;
 
 
 
    double AngularVel = 0;
 
 
 
    FString ToString() const
 
    {
 
        return FString::Printf(
 
            TEXT("Pos: %lf[rad or m] LinearVel: %lf[m/s] AngularVel: %lf[rad/s]"), Position, LinearVel, AngularVel);
 
    }
 
 
 
    bool operator==(const FRRRobotJointValue& Other) const
 
    {
 
        return FMath::IsNearlyEqual(Position, Other.Position, KINDA_SMALL_NUMBER) &&
 
               FMath::IsNearlyEqual(LinearVel, Other.LinearVel, KINDA_SMALL_NUMBER) &&
 
               FMath::IsNearlyEqual(AngularVel, Other.AngularVel, KINDA_SMALL_NUMBER);
 
    }
 
 
 
    bool operator!=(const FRRRobotJointValue& Other) const
 
    {
 
        return (false == (*this == Other));
 
    }
 
};
 
 
 
// [ROBOT LINK] --
 
//
 
// Note: Nested struct does not support USTRUCT(), and so forth also could not affort UPROPERTY members
 
// A structure representing a SDF/URDF Link
 
// http://sdformat.org/spec?ver=1.6&elem=link
 
// http://wiki.ros.org/urdf/XML/link
 
 
 
 
struct RAPYUTASIMULATIONPLUGINS_API FRRRobotLinkInertia
 
{
 
 
 
    float Mass = 0.f;
 
 
 
    FVector Location = FVector::ZeroVector;
 
 
    FQuat Rotation = FQuat::Identity;
 
 
    float Ixx = 0.f;
 
 
    float Ixy = 0.f;
 
 
    float Ixz = 0.f;
 
 
    float Iyy = 0.f;
 
 
    float Iyz = 0.f;
 
 
    float Izz = 0.f;
 
};
 
 
 
 
struct RAPYUTASIMULATIONPLUGINS_API FRREntityGeometryInfo
 
{
 
 
    FString Name;
 
 
    FString MeshName;
 
 
 
 
 
 
    FVector Size = FVector::ZeroVector;
 
 
 
 
    FVector WorldScale = FVector::OneVector;
 
 
 
 
 
    ERRShapeType LinkType = ERRShapeType::NONE;
 
 
 
 
    FString LinkName;
 
 
    FVector Location = FVector::ZeroVector;
 
 
    FQuat Rotation = FQuat::Identity;
 
    FTransform GetTransformOffset() const
 
    {
 
        return FTransform(Rotation, Location);
 
    }
 
 
    FRRMaterialProperty MaterialInfo;
 
};
 
 
 
struct RAPYUTASIMULATIONPLUGINS_API FRRSensorBaseInfo
 
{
 
 
 
    virtual ~FRRSensorBaseInfo()
 
    {
 
    }
 
 
 
    FString TopicName = TEXT("sensor_data");
 
 
 
 
 
    FString FrameId = TEXT("sensor_frame");
 
 
 
 
 
    float PublicationFrequencyHz = 0.f;
 
 
 
    virtual void PrintSelf() const
 
    {
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- TopicName: %s"), *TopicName);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- FrameId: %s"), *FrameId);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- PublicationFrequencyHz: %f"), PublicationFrequencyHz);
 
    }
 
};
 
 
 
struct RAPYUTASIMULATIONPLUGINS_API FRRSensorLidarInfo : public FRRSensorBaseInfo
 
{
 
 
    ERRLidarSensorType LidarType = ERRLidarSensorType::NONE;
 
 
 
 
 
    int32 NHorSamplesPerScan = 360;
 
 
 
 
 
    int32 NVerSamplesPerScan = 360;
 
 
 
 
 
    float HMinAngle = 0.f;
 
 
 
    float HMaxAngle = 0.f;
 
 
 
    float HResolution = 0.f;
 
 
 
    float VMinAngle = 0.f;
 
 
 
    float VMaxAngle = 0.f;
 
 
 
    float VResolution = 0.f;
 
 
 
    // Range
 
 
 
    float MinRange = 0.f;
 
 
 
    float MaxRange = 0.f;
 
 
 
    float RangeResolution = 0.f;
 
 
 
    // Noise
 
 
    FString NoiseTypeName;
 
 
    double NoiseMean = 0;
 
 
    double NoiseStdDev = 0;
 
 
 
    virtual void PrintSelf() const override
 
    {
 
        Super::PrintSelf();
 
        UE_LOG_WITH_INFO(
 
            LogTemp, Display, TEXT("- LidarType: %s"), *URRTypeUtils::GetEnumValueAsString(TEXT("ERRLidarSensorType"), LidarType));
 
 
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Scan horizontal"));
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("\t+ NHorSamplesPerScan: %d"), NHorSamplesPerScan);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("\t+ HMinAngle: %f"), HMinAngle);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("\t+ HMaxAngle: %f"), HMaxAngle);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("\t+ HResolution: %f"), HResolution);
 
 
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Scan vertical"));
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("\t+ NVerSamplesPerScan: %d"), NVerSamplesPerScan);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("\t+ VMinAngle: %f"), VMinAngle);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("\t+ VMaxAngle: %f"), VMaxAngle);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("\t+ VResolution: %f"), VResolution);
 
 
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Range"));
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("\t+ MinRange: %f"), MinRange);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("\t+ MinRange: %f"), MaxRange);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("\t+ Resolution: %f"), RangeResolution);
 
 
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Noise"));
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("\t+ Type: %s"), *NoiseTypeName);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("\t+ Mean: %f"), NoiseMean);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("\t+ StdDev: %f"), NoiseStdDev);
 
    }
 
};
 
 
 
 
struct RAPYUTASIMULATIONPLUGINS_API FRRSensorProperty
 
{
 
 
 
 
    FString LinkName;
 
 
    FString SensorName;
 
 
    ERRSensorType SensorType = ERRSensorType::NONE;
 
 
 
 
 
 
    FRRSensorLidarInfo LidarInfo;
 
 
 
    void PrintSelf() const
 
    {
 
        UE_LOG_WITH_INFO(LogTemp,
 
                         Warning,
 
                         TEXT("Sensor: %s %s"),
 
                         *URRTypeUtils::GetEnumValueAsString(TEXT("ERRSensorType"), SensorType),
 
                         *SensorName);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Link: %s"), *LinkName);
 
 
 
        LidarInfo.PrintSelf();
 
    }
 
};
 
 
 
 
struct RAPYUTASIMULATIONPLUGINS_API FRRRobotLinkProperty
 
{
 
public:
 
    FRRRobotLinkProperty()
 
    {
 
    }
 
    FRRRobotLinkProperty(FString InName) : Name(MoveTemp(InName))
 
    {
 
    }
 
 
    FString Name;
 
 
 
 
 
    FString ParentFrameName;
 
 
 
 
    int8 LinkIndex = INDEX_NONE;
 
 
    int8 ParentLinkIndex = INDEX_NONE;
 
 
 
 
 
 
 
    FVector Location = FVector::ZeroVector;
 
 
 
 
    FQuat Rotation = FQuat::Identity;
 
    FTransform GetRelativeTransformToParent() const
 
    {
 
        return FTransform(Rotation, Location);
 
    }
 
 
    FRRRobotLinkInertia Inertia;
 
 
 
 
 
 
 
    TArray<FRREntityGeometryInfo> VisualList;
 
    FTransform GetVisualOffset(int32 InVisualIndex = 0) const
 
    {
 
        return VisualList.IsValidIndex(InVisualIndex) ? VisualList[InVisualIndex].GetTransformOffset() : FTransform::Identity;
 
    }
 
    FTransform GetVisualRelativeTransformToParent(int32 InVisualIndex = 0) const
 
    {
 
        return GetRelativeTransformToParent() * GetVisualOffset(InVisualIndex);
 
    }
 
 
    TArray<FRREntityGeometryInfo> CollisionList;
 
    bool operator==(const FRRRobotLinkProperty& LinkProp)
 
    {
 
        return Name.Equals(LinkProp.Name);
 
    }
 
    bool operator==(const FString& String)
 
    {
 
        return Name.Equals(String);
 
    }
 
    // Sensor
 
 
    TArray<FRRSensorProperty> SensorList;
 
    void PrintSelf() const
 
    {
 
        UE_LOG_WITH_INFO(LogTemp, Warning, TEXT("Link Name: %s -> ParentFrameName: %s"), *Name, *ParentFrameName);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Location: %s"), *Location.ToString());
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Rotation: %s - %s"), *Rotation.ToString(), *FRotator(Rotation).ToString());
 
        // Visuals
 
        for (const auto& visual : VisualList)
 
        {
 
            UE_LOG_WITH_INFO(LogTemp, Warning, TEXT("Visual: %s"), *visual.Name);
 
            UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Link: %s"), *visual.LinkName);
 
            UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- MeshName: %s"), *visual.MeshName);
 
            UE_LOG_WITH_INFO(
 
                LogTemp, Display, TEXT("- Size: %s - WorldScale: %s"), *visual.Size.ToString(), *visual.WorldScale.ToString());
 
            UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Location: %s"), *visual.Location.ToString());
 
            UE_LOG_WITH_INFO(
 
                LogTemp, Display, TEXT("- Rotation: %s - %s"), *visual.Rotation.ToString(), *FRotator(visual.Rotation).ToString());
 
            visual.MaterialInfo.PrintSelf();
 
        }
 
        // Collisions
 
        for (const auto& collision : CollisionList)
 
        {
 
            UE_LOG_WITH_INFO(LogTemp, Warning, TEXT("Collision: %s"), *collision.Name);
 
            UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Link: %s"), *collision.LinkName);
 
            UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- MeshName: %s"), *collision.MeshName);
 
            UE_LOG_WITH_INFO(LogTemp,
 
                             Display,
 
                             TEXT("- Size: %s - WorldScale: %s"),
 
                             *collision.Size.ToString(),
 
                             *collision.WorldScale.ToString());
 
            UE_LOG_WITH_INFO(LogTemp, Display, TEXT("- Location: %s"), *collision.Location.ToString());
 
            UE_LOG_WITH_INFO(LogTemp,
 
                             Display,
 
                             TEXT("- Rotation: %s - %s"),
 
                             *collision.Rotation.ToString(),
 
                             *FRotator(collision.Rotation).ToString());
 
        }
 
        // Sensors
 
        for (const auto& sensor : SensorList)
 
        {
 
            sensor.PrintSelf();
 
        }
 
    }
 
};
 
 
 
// Ref: UChaosVehicleWheel
 
 
 
struct RAPYUTASIMULATIONPLUGINS_API FRRRobotWheelProperty
 
{
 
public:
 
    FRRRobotWheelProperty()
 
    {
 
    }
 
    FRRRobotWheelProperty(FString InWheelName) : WheelName(MoveTemp(InWheelName))
 
    {
 
    }
 
 
 
 
    FString WheelName;
 
 
 
 
 
 
    EAxleType AxleType = EAxleType::Undefined;
 
 
 
 
 
 
 
    FVector Offset = FVector::ZeroVector;
 
 
 
 
 
    float WheelRadius = 0.f;
 
 
 
 
 
    float WheelWidth = 0.f;
 
 
 
 
    float CorneringStiffness = 1000.f;
 
 
 
 
    float FrictionForceMultiplier = 2.f;
 
 
 
 
 
    float SideSlipModifier = 1.f;
 
 
 
 
 
    float SlipThreshold = 20.f;
 
 
 
 
 
    float SkidThreshold = 20.f;
 
 
 
 
    bool bAffectedBySteering = false;
 
 
 
 
    bool bAffectedByBrake = true;
 
 
 
 
    bool bAffectedByHandbrake = false;
 
 
 
 
 
    bool bAffectedByEngine = false;
 
 
 
 
 
    bool bABSEnabled = false;
 
 
 
 
 
    bool bTractionControlEnabled = false;
 
 
 
 
 
    ETorqueCombineMethod ExternalTorqueCombineMethod = ETorqueCombineMethod::None;
 
 
 
 
    FRuntimeFloatCurve LateralSlipGraph;
 
 
 
 
 
    FVector SuspensionAxis = FVector(0.f, 0.f, -1.f);
 
 
 
 
 
    FVector SuspensionForceOffset = FVector::ZeroVector;
 
 
 
 
 
 
    float SuspensionMaxRaise = 10.f;
 
 
 
 
 
 
    float SuspensionMaxDrop = 10.f;
 
 
 
    float SuspensionDampingRatio = 0.5f;
 
 
 
 
 
 
    int8 SuspensionSmoothing = 0;
 
 
 
 
 
 
 
 
    float WheelLoadRatio = 0.5f;
 
 
 
 
 
    float SpringRate = 250.0f;
 
 
 
 
 
    float SpringPreload = 50.f;
 
 
 
 
 
    float RollbarScaling = 0.15f;
 
 
 
 
 
    ESweepShape SweepShape = ESweepShape::Raycast;
 
 
 
 
 
    ESweepType SweepType = ESweepType::SimpleSweep;
 
 
 
 
 
    float MaxSteerAngleDeg = 50.f;
 
 
 
 
 
    float MaxBrakeTorque = 1500.f;
 
 
 
 
 
 
 
    float MaxHandBrakeTorque = 3000.f;
 
 
 
    void PrintSelf() const
 
    {
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("WheelName: %s Radius %f[cm] Width %f[cm]"), *WheelName, WheelRadius, WheelWidth);
 
    }
 
};
 
 
 
// Node to represent the Robot virtually as a tree
 
 
class RAPYUTASIMULATIONPLUGINS_API URRRobotNode : public UObject
 
{
 
public:
 
 
    URRRobotNode* Parent = nullptr;
 
 
    TArray<URRRobotNode*> Children;
 
 
    FRRRobotLinkProperty LinkProp;
 
 
    FRRRobotJointProperty JointProp;
 
};
 
 
 
// [ENTITY MODEL] --
 
struct FRREntityModelData
 
{
 
public:
 
    FRREntityModelData(const TArray<FString>& InModelNameList) : ModelNameList(InModelNameList)
 
    {
 
    }
 
    FRREntityModelData(TArray<FString>&& InModelNameList) : ModelNameList(MoveTemp(InModelNameList))
 
    {
 
    }
 
    FRREntityModelData()
 
    {
 
    }
 
 
 
 
 
    ERREntityDescriptionType ModelDescType = ERREntityDescriptionType::NONE;
 
    bool IsURDF() const
 
    {
 
        return (ERREntityDescriptionType::URDF == ModelDescType);
 
    }
 
    bool IsSDF() const
 
    {
 
        return (ERREntityDescriptionType::SDF == ModelDescType);
 
    }
 
    bool IsUEDataTable() const
 
    {
 
        return (ERREntityDescriptionType::UE_DATA_TABLE == ModelDescType);
 
    }
 
    bool IsSingleCAD() const
 
    {
 
        return (ERREntityDescriptionType::SINGLE_CAD == ModelDescType);
 
    }
 
    FString GetModelDescTypeName() const
 
    {
 
        return URRTypeUtils::GetEnumValueAsString(TEXT("ERREntityDescriptionType"), ModelDescType);
 
    }
 
 
 
 
    FString WorldName;
 
    bool IsWorldModel() const
 
    {
 
        return (false == WorldName.IsEmpty());
 
    }
 
 
 
 
    bool bHasWorldJoint = false;
 
 
 
 
 
 
    TArray<FString> ModelNameList;
 
    FString GetModelName() const
 
    {
 
        return IsWorldModel() ? WorldName : ((ModelNameList.Num() > 0) ? ModelNameList[0] : EMPTY_STR);
 
    }
 
 
 
 
 
    FVector TotalSize = FVector::ZeroVector;
 
 
 
    int32 GetVisualsNum() const
 
    {
 
        // Parent visuals
 
        int32 meshesNum = 0;
 
        for (const auto& linkProp : LinkPropList)
 
        {
 
            meshesNum += linkProp.VisualList.Num();
 
        }
 
 
 
        // Child visuals
 
        for (const auto& modelInfo : ChildModelsData)
 
        {
 
            meshesNum += modelInfo.GetVisualsNum();
 
        }
 
        return meshesNum;
 
    }
 
 
 
 
 
    FString BaseFolderPath;
 
 
 
 
 
    FString DescriptionFilePath;
 
 
    int32 CreatedInstancesNum = 0;
 
 
    FString ParentFrameName;
 
 
    FTransform RelativeTransform = FTransform::Identity;
 
 
 
 
 
    int32 UEComponentTypeFlags = 0;
 
    bool IsUEComponentEnabled(const int32 InTypeMask) const
 
    {
 
        return (UEComponentTypeFlags & InTypeMask);
 
    }
 
    bool IsUEComponentEnabled(const ERRUEComponentType InTypeMask) const
 
    {
 
        return IsUEComponentEnabled(static_cast<int32>(InTypeMask));
 
    }
 
    void SetUEComponentEnabled(const int32 InTypeMask)
 
    {
 
        UEComponentTypeFlags |= InTypeMask;
 
    }
 
    void SetUEComponentEnabled(const ERRUEComponentType InTypeMask)
 
    {
 
        SetUEComponentEnabled(static_cast<int32>(InTypeMask));
 
    }
 
    bool IsPlainManipulatorModel() const
 
    {
 
        return (ERRUEComponentType::ARTICULATION_DRIVE == static_cast<ERRUEComponentType>(UEComponentTypeFlags));
 
    }
 
    bool IsPlainWheeledVehicleModel() const
 
    {
 
        return (ERRUEComponentType::WHEEL_DRIVE == static_cast<ERRUEComponentType>(UEComponentTypeFlags));
 
    }
 
    bool HasDriveComponents() const
 
    {
 
        return IsUEComponentEnabled(static_cast<int32>(ERRUEComponentType::ARTICULATION_DRIVE | ERRUEComponentType::DIFF_DRIVE |
 
                                                       ERRUEComponentType::WHEEL_DRIVE));
 
    }
 
 
 
    bool IsRobotModel() const
 
    {
 
        return (JointPropList.Num() > 0) || HasDriveComponents();
 
    }
 
 
 
    bool IsObjectModel() const
 
    {
 
        return (false == IsRobotModel());
 
    }
 
 
 
    // Link/Joint list
 
 
    FString BaseLinkName;
 
 
    TArray<FRRRobotLinkProperty> LinkPropList;
 
 
    TArray<FRRRobotJointProperty> JointPropList;
 
 
 
 
    // UPROPERTY(EditAnywhere)
 
    TArray<FRREntityModelData> ChildModelsData;
 
 
 
    void UpdateLinksLocationFromJoints()
 
    {
 
        for (const auto& jointProp : JointPropList)
 
        {
 
            for (auto& linkProp : LinkPropList)
 
            {
 
                if (linkProp.Name == jointProp.ChildLinkName)
 
                {
 
                    linkProp.Location = jointProp.Location;
 
                    break;
 
                }
 
            }
 
        }
 
    }
 
 
 
    void RemoveLinkJointProp(const FString& InLinkName)
 
    {
 
        // 1- Rem joint prop having [ParentLinkName] as [InLinkName]
 
        JointPropList.RemoveAll(
 
            [this, InLinkName](const FRRRobotJointProperty& InJointProp)
 
            {
 
                if (InJointProp.ParentLinkName == InLinkName)
 
                {
 
#if RAPYUTA_SIM_DEBUG
 
                    UE_LOG_WITH_INFO(
 
                        LogTemp, Error, TEXT("Rem joint from parent [%s] %s"), *InLinkName, *InJointProp.GetTransform().ToString());
 
#endif
 
                    return true;
 
                }
 
                return false;
 
            });
 
 
 
        // 2- Rem link prop
 
        LinkPropList.RemoveAll([this, InLinkName](const FRRRobotLinkProperty& InLinkProp)
 
                               { return (InLinkProp.Name == InLinkName); });
 
 
 
        // 2.1 - BaseLinkName
 
        if (InLinkName == BaseLinkName)
 
        {
 
            BaseLinkName = TEXT("");
 
        }
 
 
 
        // 2.2 - ArticulatedLinks/EndEffectors
 
        ArticulatedLinksNames.RemoveSwap(InLinkName);
 
        EndEffectorNames.RemoveSwap(InLinkName);
 
 
 
        // 2.3 - Wheels
 
        WheelPropList.RemoveAll([this, InLinkName](const FRRRobotWheelProperty& InWheelProp)
 
                                { return (InWheelProp.WheelName == InLinkName); });
 
    }
 
 
 
    FRRRobotLinkProperty GetBaseLinkProp() const
 
    {
 
        for (const auto& linkProp : LinkPropList)
 
        {
 
            if (linkProp.Name == BaseLinkName)
 
            {
 
                return linkProp;
 
            }
 
        }
 
        return FRRRobotLinkProperty();
 
    }
 
 
 
    FRRRobotLinkProperty GetLinkProp(const FString& InLinkName) const
 
    {
 
        for (const auto& linkProp : LinkPropList)
 
        {
 
            if (linkProp.Name == InLinkName)
 
            {
 
                return linkProp;
 
            }
 
        }
 
        return FRRRobotLinkProperty();
 
    }
 
 
 
    FRRRobotJointProperty GetJointProp(const FString& InJointName) const
 
    {
 
        for (const auto& jointProp : JointPropList)
 
        {
 
            if (jointProp.Name == InJointName)
 
            {
 
                return jointProp;
 
            }
 
        }
 
        return FRRRobotJointProperty();
 
    }
 
 
 
    FRRRobotJointProperty GetJointPropFromChildLinkName(const FString& InChildLinkName) const
 
    {
 
        for (const auto& jointProp : JointPropList)
 
        {
 
            if (jointProp.ChildLinkName == InChildLinkName)
 
            {
 
                return jointProp;
 
            }
 
        }
 
        return FRRRobotJointProperty();
 
    }
 
 
 
    FRRRobotLinkProperty FindLinkProp(int8 InLinkIndex) const
 
    {
 
        for (const auto& linkProp : LinkPropList)
 
        {
 
            if (linkProp.LinkIndex == InLinkIndex)
 
            {
 
                return linkProp;
 
            }
 
        }
 
        return FRRRobotLinkProperty();
 
    }
 
 
 
    FRRRobotJointProperty FindJointPropByLinkIndex(int8 InLinkIndex) const
 
    {
 
        const FRRRobotLinkProperty linkProp = FindLinkProp(InLinkIndex);
 
        for (const auto& jointProp : JointPropList)
 
        {
 
            if (linkProp.Name == jointProp.ChildLinkName)
 
            {
 
                return jointProp;
 
            }
 
        }
 
        return FRRRobotJointProperty();
 
    }
 
 
 
    FTransform GetLinkVisualOffset(int8 InLinkIndex) const
 
    {
 
        const FRRRobotLinkProperty& linkProp = FindLinkProp(InLinkIndex);
 
        return linkProp.GetVisualOffset();
 
    }
 
 
 
    FTransform GetLinkAbsoluteTransform(int8 InLinkIndex) const
 
    {
 
        int8 linkIndexCurrent = InLinkIndex;
 
        TArray<FRRRobotJointProperty> parentLinks;
 
 
 
        while (linkIndexCurrent != INDEX_NONE)
 
        {
 
            const FRRRobotLinkProperty linkProp = FindLinkProp(linkIndexCurrent);
 
            const FRRRobotJointProperty jointProp = FindJointPropByLinkIndex(linkIndexCurrent);
 
            parentLinks.Insert(jointProp, 0);
 
            linkIndexCurrent = linkProp.ParentLinkIndex;
 
        }
 
 
 
        FTransform resultTransform = FTransform::Identity;
 
 
 
        for (const auto& jointProp : parentLinks)
 
        {
 
            FTransform jointTransform = jointProp.bIsTransformRelative ? jointProp.GetTransform() : FTransform::Identity;
 
            resultTransform.SetLocation(resultTransform.GetLocation() +
 
                                        resultTransform.GetRotation().RotateVector(jointTransform.GetLocation()));
 
            resultTransform.SetRotation(resultTransform.GetRotation() * jointTransform.GetRotation());
 
            resultTransform.SetScale3D(resultTransform.GetScale3D() * jointTransform.GetScale3D());
 
        }
 
 
 
        resultTransform *= GetLinkVisualOffset(InLinkIndex);
 
 
 
        return resultTransform;
 
    }
 
 
 
    FTransform GetLinkRelativeTransform(int8 InLinkIndex) const
 
    {
 
        const FRRRobotJointProperty jointProp = FindJointPropByLinkIndex(InLinkIndex);
 
 
 
        if (jointProp.bIsTransformRelative)
 
        {
 
            return jointProp.GetTransform();
 
        }
 
        else
 
        {
 
            const FRRRobotLinkProperty linkProp = FindLinkProp(InLinkIndex);
 
            FTransform resultTransform = linkProp.GetVisualOffset();
 
 
 
            if (linkProp.ParentLinkIndex != INDEX_NONE)
 
            {
 
                resultTransform = GetLinkAbsoluteTransform(linkProp.ParentLinkIndex).Inverse() * resultTransform;
 
            }
 
 
 
            return resultTransform;
 
        }
 
    }
 
 
 
    FTransform GetLinkRelTransformToBase(int8 InLinkIndex) const
 
    {
 
        if (InLinkIndex <= 0)
 
        {
 
            return FTransform::Identity;
 
        }
 
 
 
        for (const auto& linkProp : LinkPropList)
 
        {
 
            if (linkProp.LinkIndex == InLinkIndex)
 
            {
 
                return GetLinkRelTransformToBase(linkProp.ParentLinkIndex) * linkProp.GetRelativeTransformToParent();
 
            }
 
        }
 
        return FTransform::Identity;
 
    }
 
 
 
    FTransform GetLinkVisualOffsetToBase(int8 InLinkIndex) const
 
    {
 
        int8 linkIndexCurrent = InLinkIndex;
 
        FTransform resultTransform = GetLinkVisualOffset(InLinkIndex);
 
 
 
        while (linkIndexCurrent != INDEX_NONE)
 
        {
 
            for (const auto& linkProp : LinkPropList)
 
            {
 
                if (linkProp.LinkIndex == linkIndexCurrent)
 
                {
 
                    resultTransform = linkProp.GetRelativeTransformToParent() * resultTransform;
 
                    linkIndexCurrent = linkProp.ParentLinkIndex;
 
 
 
                    break;
 
                }
 
            }
 
        }
 
 
 
        return resultTransform;
 
    }
 
 
 
    FRREntityGeometryInfo GetVisualInfo() const
 
    {
 
        return ((LinkPropList.Num() > 0) && (LinkPropList[0].VisualList.Num() > 0)) ? LinkPropList[0].VisualList[0]
 
                                                                                    : FRREntityGeometryInfo();
 
    }
 
 
 
    // UE-StaticMesh
 
 
    FString WholeBodyStaticMeshName;
 
 
 
    // Material
 
 
    FRRMaterialProperty WholeBodyMaterialInfo;
 
    FRRMaterialProperty GetBodyMaterialInfo() const
 
    {
 
        return WholeBodyMaterialInfo;
 
    }
 
 
 
    FRRMaterialProperty GetVisualMaterialInfo() const
 
    {
 
        return GetVisualInfo().MaterialInfo;
 
    }
 
 
 
    // Articulated link names
 
 
    TArray<FString> ArticulatedLinksNames;
 
 
 
    // Endtip
 
 
    TArray<FString> EndEffectorNames;
 
 
 
    // Wheels properties
 
 
    TArray<FRRRobotWheelProperty> WheelPropList;
 
    bool HasWheel(const FString& InWheelName) const
 
    {
 
        for (const auto& wheelProp : WheelPropList)
 
        {
 
            if (wheelProp.WheelName == InWheelName)
 
            {
 
                return true;
 
            }
 
        }
 
        return false;
 
    }
 
 
 
    bool operator==(const FRREntityModelData& OtherModelData)
 
    {
 
        return (ModelDescType == OtherModelData.ModelDescType) && (ModelNameList == OtherModelData.ModelNameList) &&
 
               DescriptionFilePath.Equals(OtherModelData.DescriptionFilePath, ESearchCase::CaseSensitive);
 
    }
 
    bool IsValid(bool bIsLogged = false) const
 
    {
 
        // NOTE: Joints num could be zero for a single-link object
 
        if (ERREntityDescriptionType::NONE == ModelDescType)
 
        {
 
            if (bIsLogged)
 
            {
 
                UE_LOG_WITH_INFO(
 
                    LogTemp, Log, TEXT("%s ModelDescType has not been set!"), *FString::Join(ModelNameList, TEXT(",")));
 
            }
 
            return false;
 
        }
 
        else if (0 == ModelNameList.Num())
 
        {
 
            // Either a world or entity model
 
            if (bIsLogged)
 
            {
 
                UE_LOG_WITH_INFO(LogTemp, Log, TEXT("%s ModelNameList EMPTY!"), *DescriptionFilePath);
 
            }
 
            return false;
 
        }
 
        else if (ModelNameList[0].IsEmpty())
 
        {
 
            if (bIsLogged)
 
            {
 
                UE_LOG_WITH_INFO(LogTemp, Log, TEXT("%s ModelName EMPTY!"), *DescriptionFilePath);
 
            }
 
            return false;
 
        }
 
        else if (DescriptionFilePath.IsEmpty())
 
        {
 
            if (bIsLogged)
 
            {
 
                UE_LOG_WITH_INFO(LogTemp, Log, TEXT("%s DescriptionFilePath EMPTY!"), *FString::Join(ModelNameList, TEXT(",")));
 
            }
 
            return false;
 
        }
 
        else if (((ERREntityDescriptionType::URDF == ModelDescType) || (ERREntityDescriptionType::SDF == ModelDescType)) &&
 
                 BaseFolderPath.IsEmpty())
 
        {
 
            if (bIsLogged)
 
            {
 
                UE_LOG_WITH_INFO(LogTemp, Log, TEXT("%s BaseFolderPath EMPTY!"), *FString::Join(ModelNameList, TEXT(",")));
 
            }
 
            return false;
 
        }
 
        else if ((false == IsSingleCAD()) && (false == IsWorldModel()) && (0 == LinkPropList.Num()))
 
        {
 
            if (bIsLogged)
 
            {
 
                UE_LOG_WITH_INFO(LogTemp, Log, TEXT("%s LinkPropList EMPTY!"), *FString::Join(ModelNameList, TEXT(",")));
 
            }
 
            return false;
 
        }
 
        else if ((0 == ChildModelsData.Num()) && (ModelNameList.Num() > 1))
 
        {
 
            if (bIsLogged)
 
            {
 
                UE_LOG_WITH_INFO(
 
                    LogTemp, Log, TEXT("[%s] This should be a single model only!"), *FString::Join(ModelNameList, TEXT(",")));
 
            }
 
            return false;
 
        }
 
        else if (false == BaseLinkName.IsEmpty())
 
        {
 
            bool bBaseLinkFound = false;
 
            for (const auto& linkProp : LinkPropList)
 
            {
 
                if (BaseLinkName == linkProp.Name)
 
                {
 
                    bBaseLinkFound = true;
 
                    break;
 
                }
 
            }
 
            if (false == bBaseLinkFound)
 
            {
 
                if (bIsLogged)
 
                {
 
                    UE_LOG_WITH_INFO(LogTemp, Log, TEXT("BaseLinkName [%s] is not a link name!"), *BaseLinkName);
 
                }
 
                return false;
 
            }
 
        }
 
        else if (ArticulatedLinksNames.Num() > 0)
 
        {
 
            bool bAllArtLinksFound = true;
 
            for (const auto& artLinkName : ArticulatedLinksNames)
 
            {
 
                bool bLinkFound = false;
 
                for (const auto& linkProp : LinkPropList)
 
                {
 
                    if (artLinkName == linkProp.Name)
 
                    {
 
                        bLinkFound = true;
 
                        break;
 
                    }
 
                }
 
                if (false == bLinkFound)
 
                {
 
                    bAllArtLinksFound = false;
 
                    if (bIsLogged)
 
                    {
 
                        UE_LOG_WITH_INFO(LogTemp, Log, TEXT("Articulated link [%s] is not a link name!"), *artLinkName);
 
                    }
 
                    break;
 
                }
 
            }
 
            if (false == bAllArtLinksFound)
 
            {
 
                return false;
 
            }
 
        }
 
        else if (EndEffectorNames.Num() > 0)
 
        {
 
            bool bAllEELinksFound = true;
 
            for (const auto& eeName : EndEffectorNames)
 
            {
 
                bool bEELinkFound = false;
 
                for (const auto& linkProp : LinkPropList)
 
                {
 
                    if (eeName == linkProp.Name)
 
                    {
 
                        bEELinkFound = true;
 
                        break;
 
                    }
 
                }
 
                if (false == bEELinkFound)
 
                {
 
                    bAllEELinksFound = false;
 
                    if (bIsLogged)
 
                    {
 
                        UE_LOG_WITH_INFO(LogTemp, Log, TEXT("EndEffector [%s] is not a link name!"), *eeName);
 
                    }
 
                    break;
 
                }
 
            }
 
            if (false == bAllEELinksFound)
 
            {
 
                return false;
 
            }
 
        }
 
        else if (WheelPropList.Num() > 0)
 
        {
 
            bool bAllWheelsFound = true;
 
            for (const auto& wheelProp : WheelPropList)
 
            {
 
                bool bWheelFound = false;
 
                for (const auto& linkProp : LinkPropList)
 
                {
 
                    if (linkProp.Name == wheelProp.WheelName)
 
                    {
 
                        bWheelFound = true;
 
                        break;
 
                    }
 
                }
 
                if (false == bWheelFound)
 
                {
 
                    bAllWheelsFound = false;
 
                    if (bIsLogged)
 
                    {
 
                        UE_LOG_WITH_INFO(LogTemp, Log, TEXT("Wheel [%s] is not a link name!"), *wheelProp.WheelName);
 
                    }
 
                    break;
 
                }
 
            }
 
            if (false == bAllWheelsFound)
 
            {
 
                return false;
 
            }
 
        }
 
        return true;
 
    }
 
    void PrintSelf() const
 
    {
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("WorldName: %s"), *WorldName);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("UEComponentTypeFlags: %d"), UEComponentTypeFlags);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("bHasWorldJoint: %d"), bHasWorldJoint);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("ModelNameList: %s"), *FString::Join(ModelNameList, TEXT(",")));
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("BaseFolderPath: %s"), *FPaths::ConvertRelativePathToFull(BaseFolderPath));
 
        UE_LOG_WITH_INFO(
 
            LogTemp, Display, TEXT("DescriptionFilePath: %s"), *FPaths::ConvertRelativePathToFull(DescriptionFilePath));
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("ParentFrameName: %s"), *ParentFrameName);
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("RelativeTransform: %s"), *RelativeTransform.ToString());
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("BaseLinkName: %s"), *BaseLinkName);
 
        for (const auto& linkProp : LinkPropList)
 
        {
 
            linkProp.PrintSelf();
 
        }
 
        for (const auto& jointProp : JointPropList)
 
        {
 
            jointProp.PrintSelf();
 
        }
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("ArticulatedLinksNames: %s"), *FString::Join(ArticulatedLinksNames, TEXT(",")));
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("EndEffectorNames: %s"), *FString::Join(EndEffectorNames, TEXT(",")));
 
        for (const auto& wheelProp : WheelPropList)
 
        {
 
            wheelProp.PrintSelf();
 
        }
 
        UE_LOG_WITH_INFO(LogTemp, Display, TEXT("WholeBodyStaticMeshName: %s"), *WholeBodyStaticMeshName);
 
        WholeBodyMaterialInfo.PrintSelf();
 
 
 
        for (const auto& modelInfo : ChildModelsData)
 
        {
 
            UE_LOG_WITH_INFO(LogTemp, Warning, TEXT("SUBMODEL"));
 
            modelInfo.PrintSelf();
 
        }
 
    }
 
};    // END FRREntityModelData
 
 
 
struct RAPYUTASIMULATIONPLUGINS_API FRREntityModelTableRow : public FTableRowBase
 
{
 
public:
 
    FRREntityModelTableRow(const FRREntityModelData& InEntityModelData) : Data(InEntityModelData)
 
    {
 
    }
 
    FRREntityModelTableRow(FRREntityModelData&& InEntityModelData) : Data(MoveTemp(InEntityModelData))
 
    {
 
    }
 
    FRREntityModelTableRow()
 
    {
 
    }
 
 
    FRREntityModelData Data;
 
};
 
 
 
struct RAPYUTASIMULATIONPLUGINS_API FRREntityModelInfo : public TSharedFromThis<FRREntityModelInfo, ESPMode::ThreadSafe>
 
{
 
public:
 
    FRREntityModelInfo(const FRREntityModelData& InEntityModelData) : Data(InEntityModelData)
 
    {
 
    }
 
    FRREntityModelInfo(FRREntityModelData&& InEntityModelData) : Data(MoveTemp(InEntityModelData))
 
    {
 
    }
 
    FRREntityModelInfo()
 
    {
 
    }
 
 
 
 
    FRREntityModelData Data;
 
 
 
    FORCEINLINE const FString& GetBaseFolderPath() const
 
    {
 
        return Data.BaseFolderPath;
 
    }
 
 
 
    FORCEINLINE const FString& GetDescriptionFilePath() const
 
    {
 
        return Data.DescriptionFilePath;
 
    }
 
 
 
    FORCEINLINE const FString GetModelName() const
 
    {
 
        return Data.GetModelName();
 
    }
 
 
 
    FORCEINLINE const FString& GetWorldName() const
 
    {
 
        return Data.WorldName;
 
    }
 
 
 
    FORCEINLINE int32 GetCreatedInstancesNum() const
 
    {
 
        return Data.CreatedInstancesNum;
 
    }
 
 
 
    FORCEINLINE int32& GetCreatedInstancesNum()
 
    {
 
        return Data.CreatedInstancesNum;
 
    }
 
 
 
    FORCEINLINE bool HasWorldJoint() const
 
    {
 
        return Data.bHasWorldJoint;
 
    }
 
    FORCEINLINE int32 GetVisualsNum() const
 
    {
 
        return Data.GetVisualsNum();
 
    }
 
 
 
 
    FORCEINLINE int32 GetUEComponentTypeFlags() const
 
    {
 
        return Data.UEComponentTypeFlags;
 
    }
 
    FORCEINLINE bool IsUEComponentEnabled(const uint8 InTypeMask) const
 
    {
 
        return Data.IsUEComponentEnabled(InTypeMask);
 
    }
 
    FORCEINLINE bool IsUEComponentEnabled(const ERRUEComponentType InTypeMask) const
 
    {
 
        return Data.IsUEComponentEnabled(InTypeMask);
 
    }
 
    FORCEINLINE bool IsPlainManipulatorModel() const
 
    {
 
        return Data.IsPlainManipulatorModel();
 
    }
 
    FORCEINLINE bool IsPlainWheeledVehicleModel() const
 
    {
 
        return Data.IsPlainWheeledVehicleModel();
 
    }
 
    FORCEINLINE bool IsRobotModel() const
 
    {
 
        return Data.IsRobotModel();
 
    }
 
    FORCEINLINE bool IsObjectModel() const
 
    {
 
        return Data.IsObjectModel();
 
    }
 
 
 
    // Link/Joint list
 
    FORCEINLINE const FString& GetBaseLinkName() const
 
    {
 
        return Data.BaseLinkName;
 
    }
 
    FORCEINLINE const TArray<FRRRobotLinkProperty>& GetLinkPropList() const
 
    {
 
        return Data.LinkPropList;
 
    }
 
    FORCEINLINE TArray<FRRRobotLinkProperty>& GetLinkPropList()
 
    {
 
        return Data.LinkPropList;
 
    }
 
    FORCEINLINE const TArray<FRRRobotJointProperty>& GetJointPropList() const
 
    {
 
        return Data.JointPropList;
 
    }
 
    FORCEINLINE TArray<FRRRobotJointProperty>& GetJointPropList()
 
    {
 
        return Data.JointPropList;
 
    }
 
    FORCEINLINE const TArray<FRREntityModelData>& GetChildModelsData() const
 
    {
 
        return Data.ChildModelsData;
 
    }
 
 
 
    FORCEINLINE void RemoveLinkJointProp(const FString& InLinkName)
 
    {
 
        Data.RemoveLinkJointProp(InLinkName);
 
    }
 
 
 
    FORCEINLINE FRRRobotLinkProperty GetBaseLinkProp() const
 
    {
 
        return Data.GetBaseLinkProp();
 
    }
 
 
 
    FORCEINLINE FRRRobotLinkProperty GetLinkProp(const FString& InLinkName) const
 
    {
 
        return Data.GetLinkProp(InLinkName);
 
    }
 
 
 
    FORCEINLINE FRRRobotJointProperty GetJointProp(const FString& InJointName) const
 
    {
 
        return Data.GetJointProp(InJointName);
 
    }
 
 
 
    FORCEINLINE FRRRobotJointProperty GetJointPropFromChildLinkName(const FString& InChildLinkName) const
 
    {
 
        return Data.GetJointPropFromChildLinkName(InChildLinkName);
 
    }
 
 
 
    FORCEINLINE const FRRRobotLinkProperty FindLinkProp(int8 InLinkIndex) const
 
    {
 
        return Data.FindLinkProp(InLinkIndex);
 
    }
 
 
 
    FORCEINLINE const FRRRobotJointProperty FindJointPropByLinkIndex(int8 InLinkIndex) const
 
    {
 
        return Data.FindJointPropByLinkIndex(InLinkIndex);
 
    }
 
 
 
    FORCEINLINE FTransform GetLinkVisualOffset(int8 InLinkIndex) const
 
    {
 
        return Data.GetLinkVisualOffset(InLinkIndex);
 
    }
 
 
 
    FORCEINLINE FTransform GetLinkAbsoluteTransform(int8 InLinkIndex) const
 
    {
 
        return Data.GetLinkAbsoluteTransform(InLinkIndex);
 
    }
 
 
 
    FORCEINLINE FTransform GetLinkRelativeTransform(int8 InLinkIndex) const
 
    {
 
        return Data.GetLinkRelativeTransform(InLinkIndex);
 
    }
 
 
 
    FORCEINLINE FTransform GetLinkRelTransformToBase(int8 InLinkIndex) const
 
    {
 
        return Data.GetLinkRelTransformToBase(InLinkIndex);
 
    }
 
 
 
    FORCEINLINE FTransform GetLinkVisualOffsetToBase(int8 InLinkIndex) const
 
    {
 
        return Data.GetLinkVisualOffsetToBase(InLinkIndex);
 
    }
 
 
 
    FORCEINLINE FRREntityGeometryInfo GetVisualInfo() const
 
    {
 
        return Data.GetVisualInfo();
 
    }
 
 
 
    // Material
 
    FORCEINLINE FRRMaterialProperty GetBodyMaterialInfo() const
 
    {
 
        return Data.GetBodyMaterialInfo();
 
    }
 
 
 
    FORCEINLINE FRRMaterialProperty GetVisualMaterialInfo() const
 
    {
 
        return Data.GetVisualMaterialInfo();
 
    }
 
 
 
    // Articulated link names
 
    FORCEINLINE const TArray<FString>& GetArticulatedLinksNames() const
 
    {
 
        return Data.ArticulatedLinksNames;
 
    }
 
 
 
    // Endtip
 
    FORCEINLINE const TArray<FString>& GetEndEffectorNames() const
 
    {
 
        return Data.EndEffectorNames;
 
    }
 
 
 
    // Wheels properties
 
    FORCEINLINE const TArray<FRRRobotWheelProperty>& GetWheelPropList() const
 
    {
 
        return Data.WheelPropList;
 
    }
 
    FORCEINLINE bool HasWheel(const FString& InWheelName) const
 
    {
 
        return Data.HasWheel(InWheelName);
 
    }
 
 
 
    FORCEINLINE bool operator==(const FRREntityModelInfo& OtherModelInfo)
 
    {
 
        return (Data == OtherModelInfo.Data);
 
    }
 
    FORCEINLINE bool IsValid(bool bIsLogged = false) const
 
    {
 
        return Data.IsValid(bIsLogged);
 
    }
 
    FORCEINLINE void PrintSelf() const
 
    {
 
        Data.PrintSelf();
 
    }
 
};
 
 
 
class RAPYUTASIMULATIONPLUGINS_API FRREntityDescriptionParser
 
{
 
public:
 
    static constexpr const int8 ELEMENT_INDEX_NONE = -1;
 
    virtual ~FRREntityDescriptionParser()
 
    {
 
    }
 
    static FString GetRealPathFromMeshName(const FString& InMeshName, const FString& InRobotModelsFolderFullPath)
 
    {
 
        // Example of Mesh name in:
 
        // - URDF: [package://pr2/meshes/shoulder_v0/shoulder_pan.dae]
 
        // - SDF: [model://pr2/meshes/shoulder_v0/shoulder_pan.dae]
 
        FString meshName(InMeshName);
 
        if (meshName.RemoveFromStart(TEXT("package://")) || meshName.RemoveFromStart(TEXT("model://")))
 
        {
 
            return FPaths::ConvertRelativePathToFull(InRobotModelsFolderFullPath, meshName);
 
        }
 
        else
 
        {
 
            return InMeshName;
 
        }
 
    }
 
    virtual bool LoadModelInfoFromXML(const FString& InXMLString, FRREntityModelInfo& OutRobotModelInfo)
 
    {
 
        return true;
 
    }
 
    virtual FRREntityModelInfo LoadModelInfoFromFile(const FString& Path)
 
    {
 
        return FRREntityModelInfo();
 
    }
 
    virtual void ClearData()
 
    {
 
    }
 
};