Skip to content

Commit

Permalink
Fix SkidSteeringDriveModel (#642)
Browse files Browse the repository at this point in the history 
* Fix SkidSteeringDriveModel
* code review: fix names and comments
* code review fixes from #609
* fix Skid import hook after changes in Skid model

---------

Signed-off-by: Jan Hanca <[email protected]>
  • Loading branch information
@jhanca-robotecai
jhanca-robotecai authored Jan 15, 2024
1 parent 542b141 commit d964488
Show file tree
Hide file tree
Showing 6 changed files with 139 additions and 144 deletions.
19 changes: 13 additions & 6 deletions Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2SkidSteeringModelHook.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
*
*/

#include <AzCore/Math/MathUtils.h>
#include <ROS2/Frame/ROS2FrameComponent.h>
#include <RobotControl/Controllers/SkidSteeringController/SkidSteeringControlComponent.h>
#include <RobotControl/ROS2RobotControlComponent.h>
Expand Down Expand Up @@ -37,8 +38,8 @@ namespace ROS2::SDFormat
const auto entityIdRight = HooksUtils::GetJointEntityId(jointNameRight, sdfModel, createdEntities);
if (entityIdLeft.IsValid() && entityIdRight.IsValid())
{
HooksUtils::EnableMotor(entityIdLeft);
HooksUtils::EnableMotor(entityIdRight);
HooksUtils::SetWheelEntity(entityIdLeft);
HooksUtils::SetWheelEntity(entityIdRight);
constexpr bool steering = false; // Skid steering model does not have any steering wheels.
constexpr bool drive = true;
configuration.m_axles.emplace_back(VehicleDynamics::Utilities::Create2WheelAxle(
Expand All @@ -48,10 +49,16 @@ namespace ROS2::SDFormat
{
AZ_Warning(
"CreateVehicleConfiguration",
false,
"Cannot find entity ID for one of the joints: %s or %s",
entityIdLeft.IsValid(),
"Cannot find left joint entity %s while creating the axle %s.",
jointNameLeft.c_str(),
jointNameRight.c_str());
tag.c_str());
AZ_Warning(
"CreateVehicleConfiguration",
entityIdRight.IsValid(),
"Cannot find right joint entity %s while creating the axle %s.",
jointNameRight.c_str(),
tag.c_str());
}
};

Expand Down Expand Up @@ -114,7 +121,7 @@ namespace ROS2::SDFormat
constexpr float epsilon = 0.001f;
AZ_Warning(
"CreateVehicleConfiguration",
fabsf(configuration.m_wheelbase - wheelSeparation->Get<float>()) < epsilon,
AZ::IsClose(configuration.m_wheelbase, wheelSeparation->Get<float>(), epsilon),
"Different wheel separation distances in one model are not supported.");
}

Expand Down
31 changes: 9 additions & 22 deletions Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SDFormatHooksUtils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ namespace ROS2::SDFormat
return AZ::EntityId();
}

void HooksUtils::EnableMotor(const AZ::EntityId& entityId)
void HooksUtils::SetWheelEntity(const AZ::EntityId& entityId)
{
AZ::Entity* entity = nullptr;
AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, entityId);
Expand All @@ -52,15 +52,12 @@ namespace ROS2::SDFormat
return;
}

// Read the SDF Settings from the Settings Registry into a local struct
SdfAssetBuilderSettings sdfBuilderSettings;
sdfBuilderSettings.LoadSettings();

if (sdfBuilderSettings.m_useArticulations)
// Enable motor in hinge joint (only if articulations are not enabled)
PhysX::EditorHingeJointComponent* jointComponent = entity->FindComponent<PhysX::EditorHingeJointComponent>();
if (jointComponent != nullptr)
{
PhysX::EditorHingeJointComponent* jointComponent = entity->FindComponent<PhysX::EditorHingeJointComponent>();
entity->Activate();
if (jointComponent != nullptr && entity->GetState() == AZ::Entity::State::Active)
if (entity->GetState() == AZ::Entity::State::Active)
{
PhysX::EditorJointRequestBus::Event(
AZ::EntityComponentIdPair(entityId, jointComponent->GetId()),
Expand All @@ -69,20 +66,10 @@ namespace ROS2::SDFormat
true);
entity->Deactivate();
}
else
{
AZ_Warning("HooksUtils", false, "Cannot enable motor in hinge joint component.");
}
}
else
{
AZ::Component* wheelComponentPtr = nullptr;
PhysX::EditorArticulationLinkComponent* articulationComponent = entity->FindComponent<PhysX::EditorArticulationLinkComponent>();
if (articulationComponent != nullptr)
{
wheelComponentPtr = HooksUtils::CreateComponent<VehicleDynamics::WheelControllerComponent>(*entity);
}
AZ_Warning("HooksUtils", wheelComponentPtr != nullptr, "Cannot create WheelControllerComponent in articulation link.");
}

// Add WheelControllerComponent
AZ::Component* wheelComponentPtr = HooksUtils::CreateComponent<VehicleDynamics::WheelControllerComponent>(*entity);
AZ_Warning("HooksUtils", wheelComponentPtr != nullptr, "Cannot create WheelControllerComponent in wheel entity.");
}
} // namespace ROS2::SDFormat
5 changes: 2 additions & 3 deletions Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SDFormatHooksUtils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,10 +44,9 @@ namespace ROS2::SDFormat
//! @return entity id (invalid id if not found)
AZ::EntityId GetJointEntityId(const std::string& jointName, const sdf::Model& sdfModel, const CreatedEntitiesMap& createdEntities);

//! Enable motor in EditorHingeJointComponent if possible or create WheelControllerComponent otherwise (when articulations are
//! used); this method shows a warning message if neither was possible
//! Enable motor in EditorHingeJointComponent if possible and create WheelControllerComponent
//! @param entityId entity id of the modified entity
void EnableMotor(const AZ::EntityId& entityId);
void SetWheelEntity(const AZ::EntityId& entityId);

//! Create a component and attach the component to the entity.
//! This method ensures that game components are wrapped into GenericComponentWrapper.
Expand Down
165 changes: 79 additions & 86 deletions Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/SkidSteeringDriveModel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,71 +44,94 @@ namespace ROS2::VehicleDynamics
{
m_config = vehicleConfig;
m_wheelsData.clear();
m_wheelColumns.clear();
m_initialized = false;
}

AZStd::tuple<VehicleDynamics::WheelControllerComponent*, AZ::Vector2, AZ::Vector3> SkidSteeringDriveModel::ProduceWheelColumn(
int wheelNumber, const AxleConfiguration& axle, const int axisCount) const
void SkidSteeringDriveModel::ComputeWheelsData()
{
const auto wheelCount = axle.m_axleWheels.size();
const auto wheelEntityId = axle.m_axleWheels[wheelNumber];
// Compute number of drive axles to scale the contribution of each wheel to vehicle's velocity
int driveAxlesCount = 0;
for (const auto& axle : m_config.m_axles)
{
if (axle.m_isDrive && axle.m_axleWheels.size() > 1)
{
driveAxlesCount++;
}
AZ_Warning(
"SkidSteeringDriveModel",
axle.m_axleWheels.size() > 1,
"Axle %s has not enough wheels (%d)",
axle.m_axleTag.c_str(),
axle.m_axleWheels.size());
}
AZ_Warning("SkidSteeringDriveModel", driveAxlesCount != 0, "Skid steering model does not have any drive wheels.");

// Find the contribution of each wheel to vehicle's velocity
for (const auto& axle : m_config.m_axles)
{
const auto wheelCount = axle.m_axleWheels.size();
if (!axle.m_isDrive || wheelCount <= 1)
{
continue;
}
for (size_t wheelId = 0; wheelId < wheelCount; wheelId++)
{
const auto data = ComputeSingleWheelData(wheelId, axle, driveAxlesCount);
if (data.wheelControllerComponentPtr != nullptr)
{
m_wheelsData.emplace_back(AZStd::move(data));
}
}
}

m_initialized = true;
}

SkidSteeringDriveModel::SkidSteeringWheelData SkidSteeringDriveModel::ComputeSingleWheelData(
const int wheelId, const AxleConfiguration& axle, const int axlesCount) const
{
SkidSteeringDriveModel::SkidSteeringWheelData out;

AZ_Assert(axle.m_wheelRadius != 0, "axle.m_wheelRadius must be non-zero");
const auto wheelEntityId = axle.m_axleWheels[wheelId];
AZ::Entity* wheelEntityPtr = nullptr;
AZ::ComponentApplicationBus::BroadcastResult(wheelEntityPtr, &AZ::ComponentApplicationRequests::FindEntity, wheelEntityId);
AZ_Assert(wheelEntityPtr, "The wheelEntity should not be null here");
auto* wheelControllerComponentPtr = Utils::GetGameOrEditorComponent<WheelControllerComponent>(wheelEntityPtr);
auto wheelData = VehicleDynamics::Utilities::GetWheelData(wheelEntityId, axle.m_wheelRadius);
AZ::Transform hingeTransform = Utilities::GetJointTransform(wheelData);
if (wheelControllerComponentPtr)
out.wheelControllerComponentPtr = Utils::GetGameOrEditorComponent<WheelControllerComponent>(wheelEntityPtr);
out.wheelData = VehicleDynamics::Utilities::GetWheelData(wheelEntityId, axle.m_wheelRadius);
if (out.wheelControllerComponentPtr)
{
const float normalizedWheelId = -1.f + 2.f * wheelNumber / (wheelCount - 1);
AZ::Vector3 axis = hingeTransform.TransformVector({ 0.f, 1.f, 0.f });
float wheelBase = normalizedWheelId * m_config.m_wheelbase;
AZ_Assert(axle.m_wheelRadius != 0, "axle.m_wheelRadius must be non-zero");
float dX = axle.m_wheelRadius / (wheelCount * axisCount);
float dPhi = axle.m_wheelRadius / (wheelBase * axisCount);
return { wheelControllerComponentPtr, AZ::Vector2{ dX, dPhi }, axis };
const auto wheelsCount = axle.m_axleWheels.size();
const float normalizedWheelId = -1.f + 2.f * wheelId / (wheelsCount - 1);
out.wheelPosition = normalizedWheelId * (m_config.m_track / 2.f);
out.dX = axle.m_wheelRadius / (wheelsCount * axlesCount);
out.dPhi = axle.m_wheelRadius / (out.wheelPosition * axlesCount * axle.m_axleWheels.size());

AZ::Transform hingeTransform = Utilities::GetJointTransform(out.wheelData);
out.axis = hingeTransform.TransformVector({ 0.f, 1.f, 0.f });
}
return { nullptr, AZ::Vector2::CreateZero(), AZ::Vector3::CreateZero() };

return out;
}

AZStd::pair<AZ::Vector3, AZ::Vector3> SkidSteeringDriveModel::GetVelocityFromModel()
{
// compute every wheel contribution to vehicle velocity
if (m_wheelColumns.empty())
if (!m_initialized)
{
for (const auto& axle : m_config.m_axles)
{
const auto wheelCount = axle.m_axleWheels.size();
if (!axle.m_isDrive || wheelCount < 1)
{
continue;
}
for (size_t wheelId = 0; wheelId < wheelCount; wheelId++)
{
const auto column = ProduceWheelColumn(wheelId, axle, m_config.m_axles.size());
if (AZStd::get<0>(column))
{
m_wheelColumns.emplace_back(column);
}
}
}
ComputeWheelsData();
}

//! accumulated contribution to vehicle's linear movements of every wheel
float d_x = 0;
float dX = 0; // accumulated contribution to vehicle's linear movements of every wheel
float dPhi = 0; // accumulated contribution to vehicle's rotational movements of every wheel

//! accumulated contribution to vehicle's rotational movements of every wheel
float d_fi = 0;

// It is basically multiplication of matrix by a vector.
for (auto& [wheel, column, axis] : m_wheelColumns)
for (const auto& wheel : m_wheelsData)
{
const float omega = axis.Dot(wheel->GetRotationVelocity());
d_x += omega * column.GetX();
d_fi += omega * column.GetY();
const float omega = wheel.axis.Dot(wheel.wheelControllerComponentPtr->GetRotationVelocity());
dX += omega * wheel.dX;
dPhi += omega * wheel.dPhi;
}

return AZStd::pair<AZ::Vector3, AZ::Vector3>{ { d_x, 0, 0 }, { 0, 0, d_fi } };
return AZStd::pair<AZ::Vector3, AZ::Vector3>{ { dX, 0, 0 }, { 0, 0, dPhi } };
}

void SkidSteeringDriveModel::ApplyState(const VehicleInputs& inputs, AZ::u64 deltaTimeNs)
Expand All @@ -117,6 +140,12 @@ namespace ROS2::VehicleDynamics
{
return;
}

if (!m_initialized)
{
ComputeWheelsData();
}

const float angularTargetSpeed = inputs.m_angularRates.GetZ();
const float linearTargetSpeed = inputs.m_speed.GetX();
const float angularAcceleration = m_limits.GetAngularAcceleration();
Expand All @@ -128,48 +157,12 @@ namespace ROS2::VehicleDynamics
angularTargetSpeed, m_currentAngularVelocity, deltaTimeNs, angularAcceleration, maxAngularVelocity);
m_currentLinearVelocity =
Utilities::ComputeRampVelocity(linearTargetSpeed, m_currentLinearVelocity, deltaTimeNs, linearAcceleration, maxLinearVelocity);
// cache PhysX Hinge component IDs
if (m_wheelsData.empty())
{
int driveAxesCount = 0;
for (const auto& axle : m_config.m_axles)
{
if (axle.m_isDrive)
{
driveAxesCount++;
}
for (const auto& wheel : axle.m_axleWheels)
{
auto wheelData = VehicleDynamics::Utilities::GetWheelData(wheel, axle.m_wheelRadius);
m_wheelsData[wheel] = wheelData;
}
AZ_Warning(
"SkidSteeringDriveModel",
axle.m_axleWheels.size() > 1,
"Axle %s has not enough wheels (%d)",
axle.m_axleTag.c_str(),
axle.m_axleWheels.size());
}
AZ_Warning("SkidSteeringDriveModel", driveAxesCount != 0, "Skid steering model does not have any drive wheels.");
}

for (const auto& axle : m_config.m_axles)
for (const auto& wheel : m_wheelsData)
{
const auto wheelCount = axle.m_axleWheels.size();
if (!axle.m_isDrive || wheelCount < 1)
{
continue;
}
for (size_t wheelId = 0; wheelId < wheelCount; wheelId++)
{
const auto& wheelEntityId = axle.m_axleWheels[wheelId];
float normalizedWheelId = -1.f + 2.f * wheelId / (wheelCount - 1);
float wheelBase = normalizedWheelId * m_config.m_wheelbase / 2.f;
AZ_Assert(axle.m_wheelRadius != 0, "axle.m_wheelRadius must be non-zero");
float wheelRate = (m_currentLinearVelocity + m_currentAngularVelocity * wheelBase) / axle.m_wheelRadius;
const auto& wheelData = m_wheelsData[wheelEntityId];
VehicleDynamics::Utilities::SetWheelRotationSpeed(wheelData, wheelRate);
}
const float wheelRate =
(m_currentLinearVelocity + m_currentAngularVelocity * wheel.wheelPosition) / wheel.wheelData.m_wheelRadius;
VehicleDynamics::Utilities::SetWheelRotationSpeed(wheel.wheelData, wheelRate);
}
}

Expand Down
44 changes: 26 additions & 18 deletions Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/SkidSteeringDriveModel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@

namespace ROS2::VehicleDynamics
{
//! A simple skid steering system implementation converting speed and steering inputs into wheel impulse and steering element torque
//! A simple skid steering system implementation converting speed and steering inputs into wheel rotation
class SkidSteeringDriveModel : public DriveModel
{
public:
Expand All @@ -29,7 +29,6 @@ namespace ROS2::VehicleDynamics

// DriveModel overrides
void Activate(const VehicleConfiguration& vehicleConfig) override;

static void Reflect(AZ::ReflectContext* context);

protected:
Expand All @@ -39,25 +38,34 @@ namespace ROS2::VehicleDynamics
AZStd::pair<AZ::Vector3, AZ::Vector3> GetVelocityFromModel() override;

private:
//! Collect all necessary data to compute the impact of the wheel on the vehicle's velocity.
//! It can be thought of as a column of the Jacobian matrix of the mechanical system. Jacobian matrix for this model is a matrix of
//! size 2 x number of wheels. This function returns elements of column that corresponds to the given wheel and cache necessary data
//! to find the wheel's rotation as a scalar.
//! @param wheelNumber - number of wheels in axis
//! Data to compute the impact of a single wheel on the vehicle's velocity and to access the wheel to get/set target values.
struct SkidSteeringWheelData
{
WheelControllerComponent* wheelControllerComponentPtr{ nullptr }; //!< Pointer to wheel controller to set/get rotation speed.
WheelDynamicsData wheelData; //!< Wheel parameters.
float wheelPosition{ 0.0f }; //!< Normalized distance between the wheel and the axle's center.
float dX{ 0.0f }; //!< Wheel's contribution to vehicle's linear movement.
float dPhi{ 0.0f }; //!< Wheel's contribution to vehicle's rotational movement.
AZ::Vector3 axis{ AZ::Vector3::CreateZero() }; //!< Rotation axis of the wheel.
};
AZStd::vector<SkidSteeringWheelData> m_wheelsData; //!< Buffer with pre-calculated wheels' data.
bool m_initialized = false; //!< Information if m_wheelsData was pre-calculated.

//! Compute all necessary data for wheels' contribution to the vehicle's velocity and store it in the buffer.
void ComputeWheelsData();

//! Compute all necessary data for a single wheel's contribution to the vehicle's velocity. It can be thought of as a column of the
//! Jacobian matrix of the mechanical system. This function returns elements of column that corresponds to the given wheel, stores
//! necessary data to find the wheel's rotation as a scalar, and stores necessary data to set this rotation.
//! @param wheelId - id of the currently processed wheel in the axle vector
//! @param axle - the wheel's axle configuration
//! @param axisCount - number of axles in the vehicle
//! @returns A tuple containing of :
//! - pointer to WheelControllerComponent (API to query for wheel's ration speed),
//! - a contribution to vehicle linear and angular velocity (elements of Jacobian matrix)
//! - the axis of wheel (to convert 3D rotation speed to given scalar
AZStd::tuple<VehicleDynamics::WheelControllerComponent*, AZ::Vector2, AZ::Vector3> ProduceWheelColumn(
int wheelNumber, const AxleConfiguration& axle, const int axisCount) const;
//! @param axlesCount - number of axles in the vehicle
//! @returns SkidSteeringWheelData structure for a single wheel
SkidSteeringWheelData ComputeSingleWheelData(const int wheelId, const AxleConfiguration& axle, const int axlesCount) const;

SkidSteeringModelLimits m_limits;
AZStd::unordered_map<AZ::EntityId, VehicleDynamics::WheelDynamicsData> m_wheelsData;
AZStd::vector<AZStd::tuple<VehicleDynamics::WheelControllerComponent*, AZ::Vector2, AZ::Vector3>> m_wheelColumns;
VehicleConfiguration m_config;
float m_currentLinearVelocity = 0.0f;
float m_currentAngularVelocity = 0.0f;
float m_currentLinearVelocity{ 0.0f };
float m_currentAngularVelocity{ 0.0f };
};
} // namespace ROS2::VehicleDynamics
Loading

0 comments on commit d964488

Please sign in to comment.