diff --git a/Content/Robots/Turtlebot3/Physics/BP_RRTurtlebotWaffle.uasset b/Content/Robots/Turtlebot3/Physics/BP_RRTurtlebotWaffle.uasset index 58d3863e..53e65f6d 100644 --- a/Content/Robots/Turtlebot3/Physics/BP_RRTurtlebotWaffle.uasset +++ b/Content/Robots/Turtlebot3/Physics/BP_RRTurtlebotWaffle.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:2e7cea08165186035538d1011b907b23db38274d9d8952c7255349f581e45686 -size 206186 +oid sha256:17f4ac7b8c18001d534e4b5d211ebdb0e197975742c78402a1a9727fea0c7126 +size 206553 diff --git a/Content/Robots/Turtlebot3/Physics/BP_TurtlebotWaffle.uasset b/Content/Robots/Turtlebot3/Physics/BP_TurtlebotWaffle.uasset index 6b1acc80..544bf696 100644 --- a/Content/Robots/Turtlebot3/Physics/BP_TurtlebotWaffle.uasset +++ b/Content/Robots/Turtlebot3/Physics/BP_TurtlebotWaffle.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:f553faa0df1f7c50f894367c4cdeebd1ce45fbf6a8b2da7af52371a0d1f5bb87 -size 115551 +oid sha256:7634590df64aa0750998f39f652cc3d2fa6454f1483c59c2532a8086ab3b3831 +size 122332 diff --git a/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp index 2582ddda..8555e26e 100644 --- a/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp +++ b/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; } diff --git a/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponentBase.cpp b/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponentBase.cpp index 2b3b8478..94438a00 100644 --- a/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponentBase.cpp +++ b/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; + } } } diff --git a/Source/RapyutaSimulationPlugins/Private/Drives/RRDifferentialDriveComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Drives/RRDifferentialDriveComponent.cpp index b8d85ce1..371760a2 100644 --- a/Source/RapyutaSimulationPlugins/Private/Drives/RRDifferentialDriveComponent.cpp +++ b/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) diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp index 932398f6..9c0723ca 100644 --- a/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp @@ -32,7 +32,7 @@ void ATurtlebotBurger::SetupWheelDrives() if (bBodyComponentsCreated && IsValid(MovementComponent)) { UDifferentialDriveComponent* diffDriveComponent = CastChecked(MovementComponent); - diffDriveComponent->SetWheels(Base_WheelLeft, Base_WheelRight); + diffDriveComponent->SetWheels(Base_WheelLeft, Base_WheelRight, WheelLeft, WheelRight); diffDriveComponent->WheelRadius = WheelRadius; diffDriveComponent->WheelSeparationHalf = WheelSeparationHalf; diffDriveComponent->SetPerimeter(); diff --git a/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h b/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h index a2ba6f34..0736e476 100644 --- a/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h +++ b/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; + }; diff --git a/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponentBase.h b/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponentBase.h index a6016437..ae7b3d56 100644 --- a/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponentBase.h +++ b/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() diff --git a/Source/RapyutaSimulationPlugins/Public/Drives/RRDifferentialDriveComponent.h b/Source/RapyutaSimulationPlugins/Public/Drives/RRDifferentialDriveComponent.h index 23c28a97..fd10b4af 100644 --- a/Source/RapyutaSimulationPlugins/Public/Drives/RRDifferentialDriveComponent.h +++ b/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;