Skip to content

Commit

Permalink
Movement Components refactor (#19)
Browse files Browse the repository at this point in the history 
* Move components duplication refactored for RobotVehicle class and AMRVehicle class
* Add StatePublisher class
* Add InversionFactor variable in RobotVehicleMovementComponent
Co-authored-by: duc <[email protected]>
  • Loading branch information
@praphulkallakuri
praphulkallakuri authored and duc committed Jan 5, 2022
1 parent acdb584 commit abf5f5c
Show file tree
Hide file tree
Showing 10 changed files with 302 additions and 196 deletions.
54 changes: 27 additions & 27 deletions Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ void UDifferentialDriveComponent::UpdateMovement(float DeltaTime)
void UDifferentialDriveComponent::UpdateOdom(float DeltaTime)
{
// need to add noise!

if (!IsOdomInitialized)
{
InitOdom();
Expand All @@ -68,20 +68,20 @@ void UDifferentialDriveComponent::UpdateOdom(float DeltaTime)
uint64 ns = (uint64)(TimeNow * 1e+09f);
OdomData.header_stamp_nanosec = static_cast<uint32>(ns - (OdomData.header_stamp_sec * 1e+09));

OdomData.header_frame_id = FString("odom");
OdomData.child_frame_id = FString("base_footprint");
OdomData.header_frame_id = FString("odom");
OdomData.child_frame_id = FString("base_footprint");

// vl and vr as computed here is ok for kinematics
// for physics, vl and vr should be computed based on the change in wheel orientation (i.e. the velocity term to be used is wheel rotations per unit time [rad/s])
// together with the wheel radius or perimeter, the displacement can be computed:
// for physics, vl and vr should be computed based on the change in wheel orientation (i.e. the velocity term to be used is
// wheel rotations per unit time [rad/s]) together with the wheel radius or perimeter, the displacement can be computed:
// vl = (left_wheel_orientation_rad_now - left_wheel_orientation_rad_previous) * perimeter / (2pi)
// vr = (right_wheel_orientation_rad_now - right_wheel_orientation_rad_previous) * perimeter / (2pi)
// 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 = Velocity.X + AngularVelocity.Z * WheelSeparationHalf;
float vr = Velocity.X - AngularVelocity.Z * WheelSeparationHalf;

// noise added as a component of vl, vr
// Gazebo links this Book here: Sigwart 2011 Autonomous Mobile Robots page:337
// seems to be Introduction to Autonomous Mobile Robots (Sigwart, Nourbakhsh, Scaramuzza)
Expand Down Expand Up @@ -119,34 +119,34 @@ void UDifferentialDriveComponent::UpdateOdom(float DeltaTime)
OdomData.twist_twist_linear.Y = 0;
OdomData.twist_twist_linear.Z = 0;


OdomData.pose_covariance.Init(0,36);
OdomData.pose_covariance[0] = 0.01;
OdomData.pose_covariance[7] = 0.01;
OdomData.pose_covariance[14] = 1000000000000.0;
OdomData.pose_covariance[21] = 1000000000000.0;
OdomData.pose_covariance[28] = 1000000000000.0;
OdomData.pose_covariance[35] = 0.01;
OdomData.twist_covariance.Init(0,36);
OdomData.twist_covariance[0] = 0.01;
OdomData.twist_covariance[7] = 0.01;
OdomData.twist_covariance[14] = 1000000000000.0;
OdomData.twist_covariance[21] = 1000000000000.0;
OdomData.twist_covariance[28] = 1000000000000.0;
OdomData.twist_covariance[35] = 0.01;
OdomData.pose_covariance.Init(0, 36);
OdomData.pose_covariance[0] = 0.01;
OdomData.pose_covariance[7] = 0.01;
OdomData.pose_covariance[14] = 1000000000000.0;
OdomData.pose_covariance[21] = 1000000000000.0;
OdomData.pose_covariance[28] = 1000000000000.0;
OdomData.pose_covariance[35] = 0.01;
OdomData.twist_covariance.Init(0, 36);
OdomData.twist_covariance[0] = 0.01;
OdomData.twist_covariance[7] = 0.01;
OdomData.twist_covariance[14] = 1000000000000.0;
OdomData.twist_covariance[21] = 1000000000000.0;
OdomData.twist_covariance[28] = 1000000000000.0;
OdomData.twist_covariance[35] = 0.01;

// UE_LOG(LogTemp, Warning, TEXT("Input:"));
// UE_LOG(LogTemp, Warning, TEXT("\tVel: %s, %s"), *Velocity.ToString(), *AngularVelocity.ToString());
// UE_LOG(LogTemp, Warning, TEXT("Odometry:"));
// UE_LOG(LogTemp, Warning, TEXT("\tOdom Positon:\t\t\t\t%f %f from %f %f (%f)"), PoseEncoderX, PoseEncoderY, dx, dy, Velocity.X);
// UE_LOG(LogTemp, Warning, TEXT("\tOdom Orientation:\t\t\t%s (%f)"), *OdomData.pose_pose_orientation.ToString(), PoseEncoderTheta);
// UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistLin:\t\t\t\t%s - %f"), *OdomData.twist_twist_linear.ToString(), OdomData.twist_twist_linear.Size());
// UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistAng:\t\t\t\t%s"), *OdomData.twist_twist_angular.ToString());
// UE_LOG(LogTemp, Warning, TEXT("\tOdom Positon:\t\t\t\t%f %f from %f %f (%f)"), PoseEncoderX, PoseEncoderY, dx, dy,
// Velocity.X); UE_LOG(LogTemp, Warning, TEXT("\tOdom Orientation:\t\t\t%s (%f)"), *OdomData.pose_pose_orientation.ToString(),
// PoseEncoderTheta); UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistLin:\t\t\t\t%s - %f"), *OdomData.twist_twist_linear.ToString(),
// OdomData.twist_twist_linear.Size()); UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistAng:\t\t\t\t%s"),
// *OdomData.twist_twist_angular.ToString());
}

void UDifferentialDriveComponent::InitMovementComponent()
void UDifferentialDriveComponent::Initialize()
{
Super::InitMovementComponent();
Super::Initialize();

if (!IsValid(WheelLeft) || !IsValid(WheelRight))
{
Expand Down
27 changes: 15 additions & 12 deletions Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,9 @@

#include "Drives/RobotVehicleMovementComponent.h"

#include "Kismet/KismetMathLibrary.h"
#include "GameFramework/Pawn.h"
#include "GameFramework/PlayerController.h"
#include "Kismet/KismetMathLibrary.h"

URobotVehicleMovementComponent::URobotVehicleMovementComponent()
{
Expand All @@ -23,7 +23,7 @@ void URobotVehicleMovementComponent::UpdateMovement(float DeltaTime)
const FQuat OldRotation = UpdatedComponent->GetComponentQuat();

FVector position = UpdatedComponent->ComponentVelocity * DeltaTime;
FQuat DeltaRotation(FVector::ZAxisVector, AngularVelocity.Z * DeltaTime);
FQuat DeltaRotation(FVector::ZAxisVector, InversionFactor * AngularVelocity.Z * DeltaTime);

DesiredRotation = OldRotation * DeltaRotation;
DesiredMovement = (OldRotation * position);
Expand All @@ -46,12 +46,12 @@ void URobotVehicleMovementComponent::InitOdom()
InitialTransform.SetTranslation(PawnOwner->GetActorLocation());
InitialTransform.SetRotation(FQuat(PawnOwner->GetActorRotation()));

PreviousTransform = FTransform(FQuat(0,0,0,1), FVector(0,0,0), FVector(1,1,1));
PreviousTransform = FTransform::Identity;

OdomData.pose_pose_position_x = 0;
OdomData.pose_pose_position_y = 0;
OdomData.pose_pose_position_z = 0;
OdomData.pose_pose_orientation = FQuat(0,0,0,1);
OdomData.pose_pose_orientation = FQuat::Identity;

// todo temporary hardcoded
OdomData.pose_covariance.Init(0, 36);
Expand Down Expand Up @@ -87,7 +87,8 @@ void URobotVehicleMovementComponent::UpdateOdom(float DeltaTime)
OdomData.header_stamp_nanosec = static_cast<uint32>(ns - (OdomData.header_stamp_sec * 1e+09));

// previous estimated data
FVector PreviousEstimatedPos = FVector(OdomData.pose_pose_position_x, OdomData.pose_pose_position_y, OdomData.pose_pose_position_z);
FVector PreviousEstimatedPos =
FVector(OdomData.pose_pose_position_x, OdomData.pose_pose_position_y, OdomData.pose_pose_position_z);
FQuat PreviousEstimatedRot = OdomData.pose_pose_orientation;

// position
Expand All @@ -100,16 +101,17 @@ void URobotVehicleMovementComponent::UpdateOdom(float DeltaTime)
FQuat Rot = InitialTransform.GetRotation().Inverse() * PawnOwner->GetActorRotation().Quaternion();
FQuat PreviousRot = PreviousTransform.GetRotation();
PreviousTransform.SetRotation(Rot);
Rot = NoiseRot.Quaternion() * PreviousEstimatedRot * PreviousRot.Inverse() * Rot;
Rot = NoiseRot.Quaternion() * PreviousEstimatedRot * PreviousRot.Inverse() * Rot;
Rot.Normalize();

OdomData.pose_pose_position_x = Pos.X;
OdomData.pose_pose_position_y = Pos.Y;
OdomData.pose_pose_position_z = Pos.Z;
OdomData.pose_pose_orientation = Rot;

OdomData.twist_twist_linear = OdomData.pose_pose_orientation.UnrotateVector( Pos-PreviousEstimatedPos ) / DeltaTime;
OdomData.twist_twist_angular = FMath::DegreesToRadians((PreviousEstimatedRot * Rot.Inverse()).GetNormalized().Euler())/DeltaTime;
OdomData.twist_twist_linear = OdomData.pose_pose_orientation.UnrotateVector(Pos - PreviousEstimatedPos) / DeltaTime;
OdomData.twist_twist_angular =
FMath::DegreesToRadians((PreviousEstimatedRot * Rot.Inverse()).GetNormalized().Euler()) / DeltaTime;

// UE_LOG(LogTemp, Warning, TEXT("Odometry:"));
// UE_LOG(LogTemp, Warning, TEXT("\tCurrent Positon:\t\t\t%s"), *PawnOwner->GetActorLocation().ToString());
Expand All @@ -123,8 +125,9 @@ void URobotVehicleMovementComponent::UpdateOdom(float DeltaTime)
// UE_LOG(LogTemp, Warning, TEXT("\tPrevious Estimated Orientation:\t%s"), *PreviousEstimatedRot.Rotator().ToString());
// UE_LOG(LogTemp, Warning, TEXT("\tOdom Orientation:\t\t\t%s"), *Rot.Rotator().ToString());
// UE_LOG(LogTemp, Warning, TEXT("\tOdom Orientation:\t\t\t%s"), *Rot.ToString());
// UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistLin:\t\t\t\t%s - %f"), *OdomData.twist_twist_linear.ToString(), OdomData.twist_twist_linear.Size());
// UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistAng:\t\t\t\t%s"), *OdomData.twist_twist_angular.ToString());
// UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistLin:\t\t\t\t%s - %f"), *OdomData.twist_twist_linear.ToString(),
// OdomData.twist_twist_linear.Size()); UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistAng:\t\t\t\t%s"),
// *OdomData.twist_twist_angular.ToString());
}

void URobotVehicleMovementComponent::TickComponent(float DeltaTime,
Expand Down Expand Up @@ -155,7 +158,7 @@ FTransform URobotVehicleMovementComponent::GetOdomTF()
return TF;
}

void URobotVehicleMovementComponent::InitMovementComponent()
void URobotVehicleMovementComponent::Initialize()
{
InitOdom();
}
}
14 changes: 8 additions & 6 deletions Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,33 +2,35 @@

#include "Robots/RobotVehicle.h"

#include "Drives/RobotVehicleMovementComponent.h"
#include "Msgs/ROS2TFMsg.h"
#include "ROS2Node.h"

ARobotVehicle::ARobotVehicle(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
void ARobotVehicle::InitializeMoveComponent()
{
RobotVehicleMoveComponent = NewObject<URobotVehicleMovementComponent>(this, TEXT("RobotVehicleMoveComponent"));
}

void ARobotVehicle::Tick(float DeltaSeconds)
{
Super::Tick(DeltaSeconds);
}

void ARobotVehicle::SetLinearVel(FVector Velocity)
void ARobotVehicle::SetLinearVel(const FVector& InLinearVelocity)
{
// We're assuming input is in meters, so convert to centimeters.
MoveComponent->Velocity = Velocity;
RobotVehicleMoveComponent->Velocity = InLinearVelocity; // 0
}

void ARobotVehicle::SetAngularVel(FVector Velocity)
void ARobotVehicle::SetAngularVel(const FVector& InAngularVelocity)
{
MoveComponent->AngularVelocity = Velocity;
RobotVehicleMoveComponent->AngularVelocity = InAngularVelocity;
}

void ARobotVehicle::BeginPlay()
{
Super::BeginPlay();
MoveComponent->InitMovementComponent();
RobotVehicleMoveComponent->Initialize();
}

void ARobotVehicle::EndPlay(const EEndPlayReason::Type EndPlayReason)
Expand Down
10 changes: 7 additions & 3 deletions Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,13 +14,19 @@ ATurtlebotBurger::ATurtlebotBurger(const FObjectInitializer& ObjectInitializer)
PrimaryActorTick.bCanEverTick = true;

Init();

}

void ATurtlebotBurger::InitializeMoveComponent()
{
Super::InitializeMoveComponent();
DifferentialDriveComponent = NewObject<UDifferentialDriveComponent>(this, TEXT("DifferentialDriveComponent"));
}

void ATurtlebotBurger::Init()
{
if (!IsInitialized)
{
InitializeMoveComponent();
Base = CreateDefaultSubobject<UStaticMeshComponent>(TEXT("Base"));
LidarSensor = CreateDefaultSubobject<UStaticMeshComponent>(TEXT("LidarSensor"));
WheelLeft = CreateDefaultSubobject<UStaticMeshComponent>(TEXT("WheelLeft"));
Expand Down Expand Up @@ -56,8 +62,6 @@ void ATurtlebotBurger::SetupWheels()
{
if (IsInitialized)
{
MoveComponent = CreateDefaultSubobject<UDifferentialDriveComponent>(TEXT("MoveComponent"));
UDifferentialDriveComponent* DifferentialDriveComponent = Cast<UDifferentialDriveComponent>(MoveComponent);
DifferentialDriveComponent->SetWheels(Base_WheelLeft, Base_WheelRight);
DifferentialDriveComponent->SetPerimeter();
}
Expand Down
51 changes: 51 additions & 0 deletions Source/RapyutaSimulationPlugins/Private/Tools/StatePublisher.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
// Copyright 2020-2021 Rapyuta Robotics Co., Ltd.

#include "Tools/StatePublisher.h"

// rclUE
#include "ROS2Node.h"

// RapyutaSimulationPlugins
#include "Tools/SimulationState.h"

void UStatePublisher::RegisterPublisher(AROS2Node* Node)
{
for (auto i = 0; i < StatesToPublish.Num(); i++)
{
Node->AddPublisher(this);
}
}

void UStatePublisher::PublishState(UROS2GenericMsg* Msg)
{
UROS2EntityStateMsg* StateMsg = Cast<UROS2EntityStateMsg>(Msg);
if ((StateMsg != nullptr) && StatesToPublish.IsValidIndex(Idx))
{
StateMsg->SetMsg(StatesToPublish[Idx]);
Idx = (Idx + 1 == StatesToPublish.Num()) ? 0 : Idx + 1; // ue4 does not seem to have a modulo operator for integers
// (it does have one for floats: FGenericPlatformMath::FMod)
check(Idx < StatesToPublish.Num());
}
}

void UStatePublisher::Bind()
{
UpdateDelegate.BindDynamic(this, &UStatePublisher::PublishState);
}

void UStatePublisher::AddEntityToPublish(const FString& InName,
const FVector& InPosition,
const FRotator& InOrientation,
const FString& InRefFrame)
{
FROSEntityState BodyState;
BodyState.name = InName;
BodyState.pose_position_x = InPosition.X;
BodyState.pose_position_y = -InPosition.Y;
BodyState.pose_position_z = InPosition.Z;
BodyState.pose_orientation = InOrientation.Quaternion();
BodyState.pose_orientation.X = -BodyState.pose_orientation.X;
BodyState.pose_orientation.Z = -BodyState.pose_orientation.Z;
BodyState.reference_frame = InRefFrame;
StatesToPublish.Add(BodyState);
}
55 changes: 27 additions & 28 deletions Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,54 +2,53 @@

#pragma once

#include <random>

#include "CoreMinimal.h"
#include "Drives/RobotVehicleMovementComponent.h"

#include "PhysicsEngine/PhysicsConstraintComponent.h"

#include <random>

#include "DifferentialDriveComponent.generated.h"

DECLARE_LOG_CATEGORY_EXTERN(LogDifferentialDriveComponent, Log, All);

UCLASS( ClassGroup=(Custom), meta=(BlueprintSpawnableComponent) )
UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent))
class RAPYUTASIMULATIONPLUGINS_API UDifferentialDriveComponent : public URobotVehicleMovementComponent
{
GENERATED_BODY()
GENERATED_BODY()

public:
UDifferentialDriveComponent();
virtual void UpdateMovement(float DeltaTime) override;
virtual void UpdateOdom(float DeltaTime) override;
UFUNCTION(BlueprintCallable)
void SetWheels(UPhysicsConstraintComponent* InWheelLeft, UPhysicsConstraintComponent* InWheelRight);
UDifferentialDriveComponent();
virtual void UpdateMovement(float DeltaTime) override;
virtual void UpdateOdom(float DeltaTime) override;

UFUNCTION(BlueprintCallable)
void SetWheels(UPhysicsConstraintComponent* InWheelLeft, UPhysicsConstraintComponent* InWheelRight);

virtual void InitMovementComponent() override;
virtual void Initialize() override;

UFUNCTION(BlueprintCallable)
void SetPerimeter();
UFUNCTION(BlueprintCallable)
void SetPerimeter();

UPROPERTY(EditAnywhere,BlueprintReadWrite)
UPhysicsConstraintComponent* WheelLeft;
UPROPERTY(EditAnywhere, BlueprintReadWrite)
UPhysicsConstraintComponent* WheelLeft;

UPROPERTY(EditAnywhere,BlueprintReadWrite)
UPhysicsConstraintComponent* WheelRight;
UPROPERTY(EditAnywhere, BlueprintReadWrite)
UPhysicsConstraintComponent* WheelRight;

UPROPERTY(EditAnywhere,BlueprintReadWrite)
float WheelRadius = 1.0f;
UPROPERTY(EditAnywhere, BlueprintReadWrite)
float WheelRadius = 1.0f;

UPROPERTY(EditAnywhere,BlueprintReadWrite)
float WheelSeparationHalf = 1.0f; //todo get data from links
UPROPERTY(EditAnywhere, BlueprintReadWrite)
float WheelSeparationHalf = 1.0f; // todo get data from links

UPROPERTY(EditAnywhere,BlueprintReadWrite)
float MaxForce = 1000; //todo get data from physics constraints
UPROPERTY(EditAnywhere, BlueprintReadWrite)
float MaxForce = 1000; // todo get data from physics constraints

private:
float WheelPerimeter = 6.28f;
float WheelPerimeter = 6.28f;

float PoseEncoderX = 0;
float PoseEncoderY = 0;
float PoseEncoderTheta = 0;
float PoseEncoderX = 0;
float PoseEncoderY = 0;
float PoseEncoderTheta = 0;
};
Loading

0 comments on commit abf5f5c

Please sign in to comment.