Skip to content

Commit 357c4fb

Browse files
yuokamotoCopilot
authored and
web-flow
committed
Port odom (#372)
* port odom calc method from RRDifferentialDriveComponent to DifferentialDriveComponent * update BP_TurtlebotWaffle for odom update * update waffle param * Update Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp Co-authored-by: Copilot <[email protected]> * Update Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp Co-authored-by: Copilot <[email protected]> --------- Co-authored-by: Copilot <[email protected]>
1 parent cca8f4f commit 357c4fb

File tree

9 files changed

+106
-23
lines changed

9 files changed

+106
-23
lines changed

‎Content/Robots/Turtlebot3/Physics/BP_RRTurtlebotWaffle.uasset‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,3 @@
11
version https://git-lfs.github.com/spec/v1
2-
oid sha256:2e7cea08165186035538d1011b907b23db38274d9d8952c7255349f581e45686
3-
size 206186
2+
oid sha256:17f4ac7b8c18001d534e4b5d211ebdb0e197975742c78402a1a9727fea0c7126
3+
size 206553

‎Content/Robots/Turtlebot3/Physics/BP_TurtlebotWaffle.uasset‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,3 @@
11
version https://git-lfs.github.com/spec/v1
2-
oid sha256:f553faa0df1f7c50f894367c4cdeebd1ce45fbf6a8b2da7af52371a0d1f5bb87
3-
size 115551
2+
oid sha256:7634590df64aa0750998f39f652cc3d2fa6454f1483c59c2532a8086ab3b3831
3+
size 122332

‎Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp‎

Lines changed: 57 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,9 @@
77

88
// RapyutaSimulationPlugins
99
#include "Core/RRConversionUtils.h"
10+
#include "Core/RRGeneralUtils.h"
1011

11-
void UDifferentialDriveComponent::SetWheels(UPhysicsConstraintComponent* InWheelLeft, UPhysicsConstraintComponent* InWheelRight)
12+
void UDifferentialDriveComponent::SetWheels(UPhysicsConstraintComponent* InWheelLeft, UPhysicsConstraintComponent* InWheelRight, UStaticMeshComponent* InWheelLeftLink, UStaticMeshComponent* InWheelRightLink)
1213
{
1314
auto fSetWheel = [this](UPhysicsConstraintComponent*& CurWheel, UPhysicsConstraintComponent* NewWheel)
1415
{
@@ -27,6 +28,25 @@ void UDifferentialDriveComponent::SetWheels(UPhysicsConstraintComponent* InWheel
2728

2829
fSetWheel(WheelLeft, InWheelLeft);
2930
fSetWheel(WheelRight, InWheelRight);
31+
if (!IsValid(InWheelLeftLink))
32+
{
33+
UE_LOG_WITH_INFO_NAMED(LogDifferentialDriveComponent, Error, TEXT("Wheel Left Link is invalid! Ensure it is properly initialized and assigned."));
34+
return;
35+
}
36+
if (!IsValid(InWheelRightLink))
37+
{
38+
UE_LOG_WITH_INFO_NAMED(LogDifferentialDriveComponent, Error, TEXT("Wheel Right Link is invalid! Ensure it is properly initialized and assigned."));
39+
return;
40+
}
41+
else
42+
{
43+
WheelLeftLink = InWheelLeftLink;
44+
WheelRightLink = InWheelRightLink;
45+
LeftJointToChildLink =
46+
URRGeneralUtils::GetRelativeTransform(WheelLeft->GetComponentTransform(), WheelLeftLink->GetComponentTransform());
47+
RightJointToChildLink =
48+
URRGeneralUtils::GetRelativeTransform(WheelRight->GetComponentTransform(), WheelRightLink->GetComponentTransform());
49+
}
3050
}
3151

3252
void UDifferentialDriveComponent::UpdateMovement(float DeltaTime)
@@ -46,21 +66,51 @@ void UDifferentialDriveComponent::UpdateMovement(float DeltaTime)
4666
}
4767
}
4868

49-
float UDifferentialDriveComponent::GetWheelVelocity(const EDiffDriveWheel WheelIndex)
69+
float UDifferentialDriveComponent::GetWheelVelocity(const EDiffDriveWheel WheelIndex, float DeltaTime)
5070
{
71+
if(!IsValid(WheelLeftLink) || !IsValid(WheelRightLink))
72+
{
73+
return 0;
74+
}
5175
// todo calculate from wheel pose
52-
const float angularVelRad = FMath::DegreesToRadians(AngularVelocity.Z);
76+
// const float angularVelRad = FMath::DegreesToRadians(AngularVelocity.Z);
5377
float out = 0;
78+
79+
FVector dummyPosition = FVector::ZeroVector;
80+
FVector angularVelocity = FVector::ZeroVector;
81+
FRotator prevOrientation = FRotator::ZeroRotator;
82+
FVector prevOrientationEuler = FVector::ZeroVector;
83+
FVector OrientationEuler = FVector::ZeroVector;
84+
5485
if (WheelIndex == EDiffDriveWheel::LEFT)
5586
{
5687
// left wheel
57-
out = Velocity.X + angularVelRad * WheelSeparationHalf; //cm
88+
// out = Velocity.X + angularVelRad * WheelSeparationHalf; //cm
89+
prevOrientation = LeftWheelOrientation;
90+
prevOrientationEuler = prevOrientation.Euler();
91+
URRGeneralUtils::GetPhysicsConstraintTransform(WheelLeft, LeftJointToChildLink, dummyPosition, LeftWheelOrientation, WheelLeftLink);
92+
OrientationEuler = LeftWheelOrientation.Euler();
93+
for (uint8 i = 0; i < 3; i++)
94+
{
95+
angularVelocity[i] =
96+
UKismetMathLibrary::SafeDivide(FRotator::NormalizeAxis(OrientationEuler[i] - prevOrientationEuler[i]), DeltaTime);
97+
}
5898
}
5999
else if (WheelIndex == EDiffDriveWheel::RIGHT)
60100
{
61101
// right wheel
62-
out = Velocity.X - angularVelRad * WheelSeparationHalf; //cm
102+
// out = Velocity.X - angularVelRad * WheelSeparationHalf; //cm
103+
prevOrientation = RightWheelOrientation;
104+
prevOrientationEuler = prevOrientation.Euler();
105+
URRGeneralUtils::GetPhysicsConstraintTransform(WheelRight, RightJointToChildLink, dummyPosition, RightWheelOrientation, WheelRightLink);
106+
OrientationEuler = RightWheelOrientation.Euler();
107+
for (uint8 i = 0; i < 3; i++)
108+
{
109+
angularVelocity[i] =
110+
- UKismetMathLibrary::SafeDivide(FRotator::NormalizeAxis(OrientationEuler[i] - prevOrientationEuler[i]), DeltaTime);
111+
}
63112
}
64-
65-
return out;
113+
// Only the first element of angularVelocity is used because it represents the rotation around the axis
114+
// relevant to the wheel's movement (typically the X-axis in this context).
115+
return FMath::DegreesToRadians(angularVelocity[0]) * WheelRadius;
66116
}

‎Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponentBase.cpp‎

Lines changed: 14 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ void UDifferentialDriveComponentBase::TickComponent(float InDeltaTime,
2525
FActorComponentTickFunction* ThisTickFunction)
2626
{
2727
Super::TickComponent(InDeltaTime, TickType, ThisTickFunction);
28-
if (!ShouldSkipUpdate(InDeltaTime))
28+
if (!ShouldSkipUpdate(InDeltaTime) && bUseWheelOdom)
2929
{
3030
UpdateOdom(InDeltaTime);
3131
}
@@ -36,7 +36,7 @@ void UDifferentialDriveComponentBase::UpdateMovement(float DeltaTime)
3636
UE_LOG_WITH_INFO_SHORT(LogRapyutaCore, Error, TEXT("This method should be implemented in child class."));
3737
}
3838

39-
float UDifferentialDriveComponentBase::GetWheelVelocity(const EDiffDriveWheel WheelIndex)
39+
float UDifferentialDriveComponentBase::GetWheelVelocity(const EDiffDriveWheel WheelIndex, float DeltaTime)
4040
{
4141
UE_LOG_WITH_INFO_SHORT(LogRapyutaCore, Error, TEXT("This method should be implemented in child class."));
4242
return 0;
@@ -70,8 +70,8 @@ void UDifferentialDriveComponentBase::UpdateOdom(float DeltaTime)
7070
// in the kinematics case, (dx,dy,dtheta) can be simplified considerably
7171
// but as this is not a performance bottleneck, for the moment we leave the full general formulation,
7272
// at least until the odom for the physics version of the agent is implemented, so that we have a reference
73-
float vl = GetWheelVelocity(EDiffDriveWheel::LEFT);
74-
float vr = GetWheelVelocity(EDiffDriveWheel::RIGHT);
73+
float vl = GetWheelVelocity(EDiffDriveWheel::LEFT, DeltaTime);
74+
float vr = GetWheelVelocity(EDiffDriveWheel::RIGHT, DeltaTime);
7575

7676
// noise added as a component of vl, vr
7777
// Gazebo links this Book here: Sigwart 2011 Autonomous Mobile Robots page:337
@@ -151,7 +151,15 @@ void UDifferentialDriveComponentBase::Initialize()
151151
SetPerimeter();
152152
if (OdomComponent)
153153
{
154-
// Odom update is done by this class instead of OdomComponent.
155-
OdomComponent->bManualUpdate = true;
154+
if (bUseWheelOdom)
155+
{
156+
// Odom update is done by this class instead of OdomComponent.
157+
OdomComponent->bManualUpdate = true;
158+
}
159+
else
160+
{
161+
// Odom update is done by original OdomComponent.
162+
OdomComponent->bManualUpdate = false;
163+
}
156164
}
157165
}

‎Source/RapyutaSimulationPlugins/Private/Drives/RRDifferentialDriveComponent.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ void URRDifferentialDriveComponent::UpdateMovement(float DeltaTime)
4444
}
4545
}
4646

47-
float URRDifferentialDriveComponent::GetWheelVelocity(const EDiffDriveWheel WheelIndex)
47+
float URRDifferentialDriveComponent::GetWheelVelocity(const EDiffDriveWheel WheelIndex, float DeltaTime)
4848
{
4949
float out = 0;
5050
if (WheelIndex == EDiffDriveWheel::LEFT)

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@ void ATurtlebotBurger::SetupWheelDrives()
3232
if (bBodyComponentsCreated && IsValid(MovementComponent))
3333
{
3434
UDifferentialDriveComponent* diffDriveComponent = CastChecked<UDifferentialDriveComponent>(MovementComponent);
35-
diffDriveComponent->SetWheels(Base_WheelLeft, Base_WheelRight);
35+
diffDriveComponent->SetWheels(Base_WheelLeft, Base_WheelRight, WheelLeft, WheelRight);
3636
diffDriveComponent->WheelRadius = WheelRadius;
3737
diffDriveComponent->WheelSeparationHalf = WheelSeparationHalf;
3838
diffDriveComponent->SetPerimeter();

‎Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h‎

Lines changed: 21 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -48,18 +48,37 @@ class RAPYUTASIMULATIONPLUGINS_API UDifferentialDriveComponent : public UDiffere
4848
* @param InWheelRight
4949
*/
5050
UFUNCTION(BlueprintCallable)
51-
void SetWheels(UPhysicsConstraintComponent* InWheelLeft, UPhysicsConstraintComponent* InWheelRight);
51+
void SetWheels(UPhysicsConstraintComponent* InWheelLeft, UPhysicsConstraintComponent* InWheelRight, UStaticMeshComponent* InWheelLeftLink, UStaticMeshComponent* InWheelRightLink);
5252

5353
/**
5454
* @brief Get the Wheel Velocity [cm/s]
5555
*
5656
* @param index index of wheels
5757
*/
58-
virtual float GetWheelVelocity(const EDiffDriveWheel WheelIndex) override;
58+
virtual float GetWheelVelocity(const EDiffDriveWheel WheelIndex, float DeltaTime) override;
5959

6060
UPROPERTY(EditAnywhere, BlueprintReadWrite)
6161
UPhysicsConstraintComponent* WheelLeft = nullptr;
6262

63+
UPROPERTY(EditAnywhere, BlueprintReadWrite)
64+
UStaticMeshComponent* WheelLeftLink = nullptr;
65+
66+
UPROPERTY(VisibleAnywhere, BlueprintReadWrite)
67+
FTransform LeftJointToChildLink = FTransform::Identity;
68+
69+
UPROPERTY(VisibleAnywhere, BlueprintReadWrite)
70+
FRotator LeftWheelOrientation = FRotator::ZeroRotator;
71+
6372
UPROPERTY(EditAnywhere, BlueprintReadWrite)
6473
UPhysicsConstraintComponent* WheelRight = nullptr;
74+
75+
UPROPERTY(EditAnywhere, BlueprintReadWrite)
76+
UStaticMeshComponent* WheelRightLink = nullptr;
77+
78+
UPROPERTY(VisibleAnywhere, BlueprintReadWrite)
79+
FTransform RightJointToChildLink = FTransform::Identity;
80+
81+
UPROPERTY(VisibleAnywhere, BlueprintReadWrite)
82+
FRotator RightWheelOrientation = FRotator::ZeroRotator;
83+
6584
};

‎Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponentBase.h‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -70,7 +70,7 @@ class RAPYUTASIMULATIONPLUGINS_API UDifferentialDriveComponentBase : public URob
7070
* @param index index of wheels
7171
*/
7272
UFUNCTION(BlueprintCallable)
73-
virtual float GetWheelVelocity(const EDiffDriveWheel WheelIndex);
73+
virtual float GetWheelVelocity(const EDiffDriveWheel WheelIndex, float DeltaTime);
7474

7575
/**
7676
* @brief Call Super::Initialize() and #SetPerimeter.
@@ -97,6 +97,12 @@ class RAPYUTASIMULATIONPLUGINS_API UDifferentialDriveComponentBase : public URob
9797
UPROPERTY(EditAnywhere, BlueprintReadWrite)
9898
float MaxForce = 1000.f;
9999

100+
//! if this is false, use original OdomComponent to update odom
101+
//! if this is true, use this class's wheel odom to update odom
102+
//! This value should sync with #bManualUpdate in #URRBaseOdomComponent
103+
UPROPERTY(EditAnywhere, BlueprintReadWrite)
104+
bool bUseWheelOdom = true;
105+
100106
protected:
101107
//! [cm]
102108
UPROPERTY()

‎Source/RapyutaSimulationPlugins/Public/Drives/RRDifferentialDriveComponent.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,7 @@ class RAPYUTASIMULATIONPLUGINS_API URRDifferentialDriveComponent : public UDiffe
4646
*
4747
* @param index index of wheels
4848
*/
49-
virtual float GetWheelVelocity(const EDiffDriveWheel WheelIndex) override;
49+
virtual float GetWheelVelocity(const EDiffDriveWheel WheelIndex, float DeltaTime) override;
5050

5151
UPROPERTY(EditAnywhere, BlueprintReadWrite)
5252
URRPhysicsJointComponent* WheelLeft = nullptr;

0 commit comments

Comments
 (0)