diff --git a/Gems/ROS2/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.cpp b/Gems/ROS2/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.cpp index 0bb91685e..1f7bb1824 100644 --- a/Gems/ROS2/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.cpp +++ b/Gems/ROS2/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.cpp @@ -20,7 +20,15 @@ namespace ROS2 { if (AZ::SerializeContext* serialize = azrtti_cast(context)) { - serialize->Class()->Version(1); + serialize->Class() + ->Version(1) + ->Field("DisableLinearVelocityXOverriding", &RigidBodyTwistControlComponent::m_disableLinearVelocityXOverriding) + ->Field("DisableLinearVelocityYOverriding", &RigidBodyTwistControlComponent::m_disableLinearVelocityYOverriding) + ->Field("DisableLinearVelocityZOverriding", &RigidBodyTwistControlComponent::m_disableLinearVelocityZOverriding) + ->Field("DisableAngularVelocityXOverriding", &RigidBodyTwistControlComponent::m_disableAngularVelocityXOverriding) + ->Field("DisableAngularVelocityYOverriding", &RigidBodyTwistControlComponent::m_disableAngularVelocityYOverriding) + ->Field("DisableAngularVelocityZOverriding", &RigidBodyTwistControlComponent::m_disableAngularVelocityZOverriding); + if (AZ::EditContext* ec = serialize->GetEditContext()) { ec->Class("Rigid Body Twist Control", "Simple control through RigidBody") @@ -28,7 +36,41 @@ namespace ROS2 ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) ->Attribute(AZ::Edit::Attributes::Category, "ROS2") ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/RigidBodyTwistControl.svg") - ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/RigidBodyTwistControl.svg"); + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/RigidBodyTwistControl.svg") + ->ClassElement(AZ::Edit::ClassElements::Group, "Linear Axis Overrides") + ->Attribute(AZ::Edit::Attributes::AutoExpand, false) + ->DataElement( + AZ::Edit::UIHandlers::Default, + &RigidBodyTwistControlComponent::m_disableLinearVelocityXOverriding, + "Disable linear velocity X axis override", + "Disable overriding X axis") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &RigidBodyTwistControlComponent::m_disableLinearVelocityYOverriding, + "Disable linear velocity Y axis override", + "Disable overriding Y axis") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &RigidBodyTwistControlComponent::m_disableLinearVelocityZOverriding, + "Disable linear velocity Z axis override", + "Disable overriding Z axis") + ->ClassElement(AZ::Edit::ClassElements::Group, "Angular Axis Overrides") + ->Attribute(AZ::Edit::Attributes::AutoExpand, false) + ->DataElement( + AZ::Edit::UIHandlers::Default, + &RigidBodyTwistControlComponent::m_disableAngularVelocityXOverriding, + "Disable angular velocity X axis override", + "Disable overriding X axis") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &RigidBodyTwistControlComponent::m_disableAngularVelocityYOverriding, + "Disable angular velocity Y axis override", + "Disable overriding Y axis") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &RigidBodyTwistControlComponent::m_disableAngularVelocityZOverriding, + "Disable angular velocity Z axis override", + "Disable overriding Z axis"); } } } @@ -51,8 +93,8 @@ namespace ROS2 AZ::TickBus::Handler::BusDisconnect(); } } - - void RigidBodyTwistControlComponent::OnTick([[maybe_unused]]float deltaTime, [[maybe_unused]]AZ::ScriptTimePoint time) + + void RigidBodyTwistControlComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) { AzPhysics::SceneInterface* sceneInterface = AZ::Interface::Get(); AZ_Assert(sceneInterface, "No scene interface"); @@ -72,19 +114,48 @@ namespace ROS2 } m_bodyHandle = rigidBody->m_bodyHandle; m_sceneFinishSimHandler = AzPhysics::SceneEvents::OnSceneSimulationFinishHandler( - [this, sceneInterface]([[maybe_unused]] AzPhysics::SceneHandle sceneHandle, float fixedDeltaTime) + [this, sceneInterface]([[maybe_unused]] AzPhysics::SceneHandle sceneHandle, float fixedDeltaTime) + { + auto* rigidBody = sceneInterface->GetSimulatedBodyFromHandle(sceneHandle, m_bodyHandle); + AZ_Assert(sceneInterface, "No body found for previously given handle"); + + // Convert local steering to world frame + const AZ::Transform robotTransform = rigidBody->GetTransform(); + auto linearVelocityGlobal = robotTransform.TransformVector(m_linearVelocityLocal); + auto angularVelocityGlobal = robotTransform.TransformVector(m_angularVelocityLocal); + AZ::Vector3 linearVelocity; + AZ::Vector3 angularVelocity; + Physics::RigidBodyRequestBus::EventResult(linearVelocity, GetEntityId(), &Physics::RigidBodyRequests::GetLinearVelocity); + Physics::RigidBodyRequestBus::EventResult(angularVelocity, GetEntityId(), &Physics::RigidBodyRequests::GetAngularVelocity); + if (m_disableLinearVelocityXOverriding) + { + linearVelocityGlobal.SetX(linearVelocity.GetX()); + } + if (m_disableLinearVelocityYOverriding) + { + linearVelocityGlobal.SetY(linearVelocity.GetY()); + } + if (m_disableLinearVelocityZOverriding) + { + linearVelocityGlobal.SetZ(linearVelocity.GetZ()); + } + if (m_disableAngularVelocityXOverriding) + { + angularVelocityGlobal.SetX(angularVelocity.GetX()); + } + if (m_disableAngularVelocityYOverriding) + { + angularVelocityGlobal.SetY(angularVelocity.GetY()); + } + if (m_disableAngularVelocityZOverriding) { - auto* rigidBody = sceneInterface->GetSimulatedBodyFromHandle(sceneHandle, m_bodyHandle); - AZ_Assert(sceneInterface, "No body found for previously given handle"); + angularVelocityGlobal.SetZ(angularVelocity.GetZ()); + } - // Convert local steering to world frame - const AZ::Transform robotTransform = rigidBody->GetTransform(); - const auto linearVelocityGlobal = robotTransform.TransformVector(m_linearVelocityLocal); - const auto angularVelocityGlobal = robotTransform.TransformVector(m_angularVelocityLocal); - Physics::RigidBodyRequestBus::Event(GetEntityId(), &Physics::RigidBodyRequests::SetLinearVelocity, linearVelocityGlobal); - Physics::RigidBodyRequestBus::Event(GetEntityId(), &Physics::RigidBodyRequests::SetAngularVelocity, angularVelocityGlobal); - }, - aznumeric_cast(AzPhysics::SceneEvents::PhysicsStartFinishSimulationPriority::Components)); + Physics::RigidBodyRequestBus::Event(GetEntityId(), &Physics::RigidBodyRequests::SetLinearVelocity, linearVelocityGlobal); + Physics::RigidBodyRequestBus::Event(GetEntityId(), &Physics::RigidBodyRequests::SetAngularVelocity, angularVelocityGlobal); + }, + aznumeric_cast(AzPhysics::SceneEvents::PhysicsStartFinishSimulationPriority::Components)); sceneInterface->RegisterSceneSimulationFinishHandler(defaultSceneHandle, m_sceneFinishSimHandler); AZ::TickBus::Handler::BusDisconnect(); } diff --git a/Gems/ROS2/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.h b/Gems/ROS2/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.h index b0b721820..a81991ff9 100644 --- a/Gems/ROS2/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.h +++ b/Gems/ROS2/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.h @@ -8,9 +8,9 @@ #pragma once #include -#include -#include #include +#include +#include namespace ROS2 { //! A component with a simple handler for Twist type of control (linear and angular velocities). @@ -44,9 +44,17 @@ namespace ROS2 void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; ////////////////////////////////////////////////////////////////////////// - AZ::Vector3 m_linearVelocityLocal {AZ::Vector3::CreateZero()}; //!< Linear velocity in local frame - AZ::Vector3 m_angularVelocityLocal {AZ::Vector3::CreateZero()}; //!< Angular velocity in local frame + AZ::Vector3 m_linearVelocityLocal{ AZ::Vector3::CreateZero() }; //!< Linear velocity in local frame + AZ::Vector3 m_angularVelocityLocal{ AZ::Vector3::CreateZero() }; //!< Angular velocity in local frame AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_sceneFinishSimHandler; //!< Handler called after every physics sub-step AzPhysics::SimulatedBodyHandle m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle; //!< Handle to the body to apply velocities to + + bool m_disableLinearVelocityXOverriding = false; + bool m_disableLinearVelocityYOverriding = false; + bool m_disableLinearVelocityZOverriding = false; + + bool m_disableAngularVelocityXOverriding = false; + bool m_disableAngularVelocityYOverriding = false; + bool m_disableAngularVelocityZOverriding = false; }; } // namespace ROS2