[cherry-pick to UE5.2 from UE5.3] [cherry-pick to UE5.3 from devel] Port odom

Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp

@@ -7,8 +7,9 @@
 
 // RapyutaSimulationPlugins
 #include "Core/RRConversionUtils.h"
+#include "Core/RRGeneralUtils.h"
 
-void UDifferentialDriveComponent::SetWheels(UPhysicsConstraintComponent* InWheelLeft, UPhysicsConstraintComponent* InWheelRight)
+void UDifferentialDriveComponent::SetWheels(UPhysicsConstraintComponent* InWheelLeft, UPhysicsConstraintComponent* InWheelRight, UStaticMeshComponent* InWheelLeftLink, UStaticMeshComponent* InWheelRightLink)
 {
     auto fSetWheel = [this](UPhysicsConstraintComponent*& CurWheel, UPhysicsConstraintComponent* NewWheel)
     {
@@ -27,6 +28,25 @@ void UDifferentialDriveComponent::SetWheels(UPhysicsConstraintComponent* InWheel
 
     fSetWheel(WheelLeft, InWheelLeft);
     fSetWheel(WheelRight, InWheelRight);
+    if (!IsValid(InWheelLeftLink))
+    {
+        UE_LOG_WITH_INFO_NAMED(LogDifferentialDriveComponent, Error, TEXT("Wheel Left Link is invalid! Ensure it is properly initialized and assigned."));
+        return;
+    }
+    if (!IsValid(InWheelRightLink))
+    {
+        UE_LOG_WITH_INFO_NAMED(LogDifferentialDriveComponent, Error, TEXT("Wheel Right Link is invalid! Ensure it is properly initialized and assigned."));
+        return;
+    }
+    else
+    {
+        WheelLeftLink = InWheelLeftLink;
+        WheelRightLink = InWheelRightLink;
+        LeftJointToChildLink =
+                URRGeneralUtils::GetRelativeTransform(WheelLeft->GetComponentTransform(), WheelLeftLink->GetComponentTransform());
+        RightJointToChildLink = 
+                URRGeneralUtils::GetRelativeTransform(WheelRight->GetComponentTransform(), WheelRightLink->GetComponentTransform());
+    }
 }
 
 void UDifferentialDriveComponent::UpdateMovement(float DeltaTime)
@@ -46,21 +66,51 @@ void UDifferentialDriveComponent::UpdateMovement(float DeltaTime)
     }
 }
 
-float UDifferentialDriveComponent::GetWheelVelocity(const EDiffDriveWheel WheelIndex)
+float UDifferentialDriveComponent::GetWheelVelocity(const EDiffDriveWheel WheelIndex, float DeltaTime)
 {
+    if(!IsValid(WheelLeftLink) || !IsValid(WheelRightLink))
+    {
+        return 0;
+    }
     // todo calculate from wheel pose
-    const float angularVelRad = FMath::DegreesToRadians(AngularVelocity.Z);
+    // const float angularVelRad = FMath::DegreesToRadians(AngularVelocity.Z);
     float out = 0;
+
+    FVector dummyPosition = FVector::ZeroVector;
+    FVector angularVelocity = FVector::ZeroVector;
+    FRotator prevOrientation = FRotator::ZeroRotator;
+    FVector prevOrientationEuler = FVector::ZeroVector;
+    FVector OrientationEuler = FVector::ZeroVector;
+
     if (WheelIndex == EDiffDriveWheel::LEFT)
     {
         // left wheel
-        out = Velocity.X + angularVelRad * WheelSeparationHalf;    //cm
+        // out = Velocity.X + angularVelRad * WheelSeparationHalf;    //cm
+        prevOrientation = LeftWheelOrientation;
+        prevOrientationEuler = prevOrientation.Euler();
+        URRGeneralUtils::GetPhysicsConstraintTransform(WheelLeft, LeftJointToChildLink, dummyPosition, LeftWheelOrientation, WheelLeftLink);
+        OrientationEuler = LeftWheelOrientation.Euler();
+        for (uint8 i = 0; i < 3; i++)
+        {
+            angularVelocity[i] =
+                 UKismetMathLibrary::SafeDivide(FRotator::NormalizeAxis(OrientationEuler[i] - prevOrientationEuler[i]), DeltaTime);
+        }
     }
     else if (WheelIndex == EDiffDriveWheel::RIGHT)
     {
         // right wheel
-        out = Velocity.X - angularVelRad * WheelSeparationHalf;    //cm
+        // out = Velocity.X - angularVelRad * WheelSeparationHalf;    //cm
+        prevOrientation = RightWheelOrientation;
+        prevOrientationEuler = prevOrientation.Euler();
+        URRGeneralUtils::GetPhysicsConstraintTransform(WheelRight, RightJointToChildLink, dummyPosition, RightWheelOrientation, WheelRightLink);
+        OrientationEuler = RightWheelOrientation.Euler();
+        for (uint8 i = 0; i < 3; i++)
+        {
+            angularVelocity[i] =
+                - UKismetMathLibrary::SafeDivide(FRotator::NormalizeAxis(OrientationEuler[i] - prevOrientationEuler[i]), DeltaTime);
+        }
     }
-
-    return out;
+    // Only the first element of angularVelocity is used because it represents the rotation around the axis
+    // relevant to the wheel's movement (typically the X-axis in this context).
+    return FMath::DegreesToRadians(angularVelocity[0]) * WheelRadius;
 }

Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponentBase.cpp

@@ -25,7 +25,7 @@ void UDifferentialDriveComponentBase::TickComponent(float InDeltaTime,
                                                     FActorComponentTickFunction* ThisTickFunction)
 {
     Super::TickComponent(InDeltaTime, TickType, ThisTickFunction);
-    if (!ShouldSkipUpdate(InDeltaTime))
+    if (!ShouldSkipUpdate(InDeltaTime) && bUseWheelOdom)
     {
         UpdateOdom(InDeltaTime);
     }
@@ -36,7 +36,7 @@ void UDifferentialDriveComponentBase::UpdateMovement(float DeltaTime)
     UE_LOG_WITH_INFO_SHORT(LogRapyutaCore, Error, TEXT("This method should be implemented in child class."));
 }
 
-float UDifferentialDriveComponentBase::GetWheelVelocity(const EDiffDriveWheel WheelIndex)
+float UDifferentialDriveComponentBase::GetWheelVelocity(const EDiffDriveWheel WheelIndex, float DeltaTime)
 {
     UE_LOG_WITH_INFO_SHORT(LogRapyutaCore, Error, TEXT("This method should be implemented in child class."));
     return 0;
@@ -70,8 +70,8 @@ void UDifferentialDriveComponentBase::UpdateOdom(float DeltaTime)
     // in the kinematics case, (dx,dy,dtheta) can be simplified considerably
     // but as this is not a performance bottleneck, for the moment we leave the full general formulation,
     // at least until the odom for the physics version of the agent is implemented, so that we have a reference
-    float vl = GetWheelVelocity(EDiffDriveWheel::LEFT);
-    float vr = GetWheelVelocity(EDiffDriveWheel::RIGHT);
+    float vl = GetWheelVelocity(EDiffDriveWheel::LEFT, DeltaTime);
+    float vr = GetWheelVelocity(EDiffDriveWheel::RIGHT, DeltaTime);
 
     // noise added as a component of vl, vr
     // Gazebo links this Book here: Sigwart 2011 Autonomous Mobile Robots page:337
@@ -151,7 +151,15 @@ void UDifferentialDriveComponentBase::Initialize()
     SetPerimeter();
     if (OdomComponent)
     {
-        // Odom update is done by this class instead of OdomComponent.
-        OdomComponent->bManualUpdate = true;
+        if (bUseWheelOdom)
+        {
+            // Odom update is done by this class instead of OdomComponent.
+            OdomComponent->bManualUpdate = true;
+        }
+        else
+        {
+            // Odom update is done by original OdomComponent.
+            OdomComponent->bManualUpdate = false;
+        }
     }
 }

Source/RapyutaSimulationPlugins/Private/Drives/RRDifferentialDriveComponent.cpp

@@ -44,7 +44,7 @@ void URRDifferentialDriveComponent::UpdateMovement(float DeltaTime)
     }
 }
 
-float URRDifferentialDriveComponent::GetWheelVelocity(const EDiffDriveWheel WheelIndex)
+float URRDifferentialDriveComponent::GetWheelVelocity(const EDiffDriveWheel WheelIndex, float DeltaTime)
 {
     float out = 0;
     if (WheelIndex == EDiffDriveWheel::LEFT)

Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp

@@ -32,7 +32,7 @@ void ATurtlebotBurger::SetupWheelDrives()
     if (bBodyComponentsCreated && IsValid(MovementComponent))
     {
         UDifferentialDriveComponent* diffDriveComponent = CastChecked<UDifferentialDriveComponent>(MovementComponent);
-        diffDriveComponent->SetWheels(Base_WheelLeft, Base_WheelRight);
+        diffDriveComponent->SetWheels(Base_WheelLeft, Base_WheelRight, WheelLeft, WheelRight);
         diffDriveComponent->WheelRadius = WheelRadius;
         diffDriveComponent->WheelSeparationHalf = WheelSeparationHalf;
         diffDriveComponent->SetPerimeter();

Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h

@@ -48,18 +48,37 @@ class RAPYUTASIMULATIONPLUGINS_API UDifferentialDriveComponent : public UDiffere
      * @param InWheelRight
      */
     UFUNCTION(BlueprintCallable)
-    void SetWheels(UPhysicsConstraintComponent* InWheelLeft, UPhysicsConstraintComponent* InWheelRight);
+    void SetWheels(UPhysicsConstraintComponent* InWheelLeft, UPhysicsConstraintComponent* InWheelRight, UStaticMeshComponent* InWheelLeftLink, UStaticMeshComponent* InWheelRightLink);
 
     /**
      * @brief Get the Wheel Velocity [cm/s]
      *
      * @param index index of wheels
      */
-    virtual float GetWheelVelocity(const EDiffDriveWheel WheelIndex) override;
+    virtual float GetWheelVelocity(const EDiffDriveWheel WheelIndex, float DeltaTime) override;
 
     UPROPERTY(EditAnywhere, BlueprintReadWrite)
     UPhysicsConstraintComponent* WheelLeft = nullptr;
 
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    UStaticMeshComponent* WheelLeftLink = nullptr;
+
+    UPROPERTY(VisibleAnywhere, BlueprintReadWrite)
+    FTransform LeftJointToChildLink = FTransform::Identity;
+
+    UPROPERTY(VisibleAnywhere, BlueprintReadWrite)
+    FRotator LeftWheelOrientation = FRotator::ZeroRotator;
+
     UPROPERTY(EditAnywhere, BlueprintReadWrite)
     UPhysicsConstraintComponent* WheelRight = nullptr;
+
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    UStaticMeshComponent* WheelRightLink = nullptr;
+
+    UPROPERTY(VisibleAnywhere, BlueprintReadWrite)
+    FTransform RightJointToChildLink = FTransform::Identity;
+
+    UPROPERTY(VisibleAnywhere, BlueprintReadWrite)
+    FRotator RightWheelOrientation = FRotator::ZeroRotator;
+
 };

Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponentBase.h

@@ -70,7 +70,7 @@ class RAPYUTASIMULATIONPLUGINS_API UDifferentialDriveComponentBase : public URob
      * @param index index of wheels
      */
     UFUNCTION(BlueprintCallable)
-    virtual float GetWheelVelocity(const EDiffDriveWheel WheelIndex);
+    virtual float GetWheelVelocity(const EDiffDriveWheel WheelIndex, float DeltaTime);
 
     /**
      * @brief Call Super::Initialize() and #SetPerimeter.
@@ -97,6 +97,12 @@ class RAPYUTASIMULATIONPLUGINS_API UDifferentialDriveComponentBase : public URob
     UPROPERTY(EditAnywhere, BlueprintReadWrite)
     float MaxForce = 1000.f;
 
+    //! if this is false, use original OdomComponent to update odom
+    //! if this is true, use this class's wheel odom to update odom
+    //! This value should sync with #bManualUpdate in #URRBaseOdomComponent
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    bool bUseWheelOdom = true;
+
 protected:
     //! [cm]
     UPROPERTY()

Source/RapyutaSimulationPlugins/Public/Drives/RRDifferentialDriveComponent.h

@@ -46,7 +46,7 @@ class RAPYUTASIMULATIONPLUGINS_API URRDifferentialDriveComponent : public UDiffe
      *
      * @param index index of wheels
      */
-    virtual float GetWheelVelocity(const EDiffDriveWheel WheelIndex) override;
+    virtual float GetWheelVelocity(const EDiffDriveWheel WheelIndex, float DeltaTime) override;
 
     UPROPERTY(EditAnywhere, BlueprintReadWrite)
     URRPhysicsJointComponent* WheelLeft = nullptr;