123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342 |
- /*
- * Copyright (c) Contributors to the Open 3D Engine Project.
- * For complete copyright and license terms please see the LICENSE at the root
- * of this distribution.
- *
- * SPDX-License-Identifier: Apache-2.0 OR MIT
- *
- */
- #include "VacuumGripperComponent.h"
- #include "Source/ArticulationLinkComponent.h"
- #include "Utils.h"
- #include <Utilities/ArticulationsUtilities.h>
- #include <AzCore/Serialization/EditContext.h>
- #include <AzCore/Serialization/SerializeContext.h>
- #include <AzFramework/Components/TransformComponent.h>
- #include <AzFramework/Physics/PhysicsSystem.h>
- #include <AzFramework/Physics/RigidBodyBus.h>
- #include <AzFramework/Physics/SimulatedBodies/RigidBody.h>
- #include <imgui/imgui.h>
- namespace ROS2
- {
- void VacuumGripperComponent::Activate()
- {
- m_gripperEffectorBodyHandle = AzPhysics::InvalidSimulatedBodyHandle;
- m_onTriggerEnterHandler = AzPhysics::SimulatedBodyEvents::OnTriggerEnter::Handler(
- [&]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, [[maybe_unused]] const AzPhysics::TriggerEvent& event)
- {
- const auto grippedEntityCandidateId = event.m_otherBody->GetEntityId();
- const bool isGrippable = isObjectGrippable(grippedEntityCandidateId);
- if (isGrippable)
- {
- m_grippedObjectInEffector = grippedEntityCandidateId;
- }
- });
- m_onTriggerExitHandler = AzPhysics::SimulatedBodyEvents::OnTriggerExit::Handler(
- [&]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, [[maybe_unused]] const AzPhysics::TriggerEvent& event)
- {
- const auto grippedEntityCandidateId = event.m_otherBody->GetEntityId();
- if (m_grippedObjectInEffector == grippedEntityCandidateId)
- {
- m_grippedObjectInEffector = AZ::EntityId(AZ::EntityId::InvalidEntityId);
- }
- });
- m_vacuumJoint = AzPhysics::InvalidJointHandle;
- m_grippedObjectInEffector = AZ::EntityId(AZ::EntityId::InvalidEntityId);
- m_tryingToGrip = false;
- m_cancelGripperCommand = false;
- AZ::TickBus::Handler::BusConnect();
- ImGui::ImGuiUpdateListenerBus::Handler::BusConnect();
- GripperRequestBus::Handler::BusConnect(GetEntityId());
- }
- void VacuumGripperComponent::Deactivate()
- {
- m_onTriggerEnterHandler.Disconnect();
- m_onTriggerExitHandler.Disconnect();
- GripperRequestBus::Handler::BusDisconnect();
- AZ::TickBus::Handler::BusDisconnect();
- ImGui::ImGuiUpdateListenerBus::Handler::BusDisconnect();
- }
- void VacuumGripperComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
- {
- provided.push_back(AZ_CRC_CE("GripperService"));
- }
- void VacuumGripperComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
- {
- incompatible.push_back(AZ_CRC_CE("GripperService"));
- }
- void VacuumGripperComponent::Reflect(AZ::ReflectContext* context)
- {
- if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
- {
- serialize->Class<VacuumGripperComponent, AZ::Component>()
- ->Field("EffectorCollider", &VacuumGripperComponent::m_gripperEffectorCollider)
- ->Field("EffectorArticulation", &VacuumGripperComponent::m_gripperEffectorArticulationLink)
- ->Version(1);
- if (AZ::EditContext* ec = serialize->GetEditContext())
- {
- ec->Class<VacuumGripperComponent>("VacuumGripperComponent", "Component for control of a vacuum gripper.")
- ->ClassElement(AZ::Edit::ClassElements::EditorData, "VacuumGripper")
- ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
- ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
- ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/VacuumGripperComponent.svg")
- ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/VacuumGripperComponent.svg")
- ->DataElement(
- AZ::Edit::UIHandlers::EntityId,
- &VacuumGripperComponent::m_gripperEffectorCollider,
- "Effector Trigger Collider",
- "The entity with trigger collider that will detect objects that can be successfully gripped.")
- ->DataElement(
- AZ::Edit::UIHandlers::EntityId,
- &VacuumGripperComponent::m_gripperEffectorArticulationLink,
- "Effector Articulation Link",
- "The entity that is the articulation link of the effector.");
- }
- }
- }
- void VacuumGripperComponent::OnTick(float delta, AZ::ScriptTimePoint timePoint)
- {
- AZ_Assert(AZ::Interface<AzPhysics::SystemInterface>::Get(), "No physics system.");
- AzPhysics::SceneInterface* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
- AZ_Assert(sceneInterface, "No scene intreface.");
- AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
- AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle.");
- // Connect the trigger handlers if not already connected, it is circumventing the issue GH-16188, the
- // RigidbodyNotificationBus should be used instead.
- if (!m_onTriggerEnterHandler.IsConnected() || !m_onTriggerExitHandler.IsConnected())
- {
- if (auto* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get())
- {
- AZStd::pair<AzPhysics::SceneHandle, AzPhysics::SimulatedBodyHandle> foundBody =
- physicsSystem->FindAttachedBodyHandleFromEntityId(m_gripperEffectorCollider);
- AZ_Warning(
- "VacuumGripper", foundBody.first != AzPhysics::InvalidSceneHandle, "No body found for m_gripperEffectorCollider.");
- if (foundBody.first != AzPhysics::InvalidSceneHandle)
- {
- AzPhysics::SimulatedBodyEvents::RegisterOnTriggerEnterHandler(
- foundBody.first, foundBody.second, m_onTriggerEnterHandler);
- AzPhysics::SimulatedBodyEvents::RegisterOnTriggerExitHandler(foundBody.first, foundBody.second, m_onTriggerExitHandler);
- }
- }
- AZ::EntityId rootArticulationEntity = Utils::GetRootOfArticulation(m_gripperEffectorArticulationLink);
- AZ::Entity* rootEntity = nullptr;
- AZ::ComponentApplicationBus::BroadcastResult(rootEntity, &AZ::ComponentApplicationRequests::FindEntity, rootArticulationEntity);
- AZ_Trace("VacuumGripper", "Root articulation entity name: %s\n", rootEntity->GetName().c_str());
- PhysX::ArticulationLinkComponent* component = rootEntity->FindComponent<PhysX::ArticulationLinkComponent>();
- AZStd::vector<AzPhysics::SimulatedBodyHandle> articulationHandles = component->GetSimulatedBodyHandles();
- AZ_Assert(articulationHandles.size() > 1, "Expected more than one body handles in articulations");
- for (AzPhysics::SimulatedBodyHandle handle : articulationHandles)
- {
- AzPhysics::SimulatedBody* body = sceneInterface->GetSimulatedBodyFromHandle(defaultSceneHandle, handle);
- AZ_Assert(body, "Expected valid body pointer");
- if (body->GetEntityId() == m_gripperEffectorArticulationLink)
- {
- m_gripperEffectorBodyHandle = handle;
- }
- }
- }
- if (m_tryingToGrip)
- {
- TryToGripObject();
- }
- }
- bool VacuumGripperComponent::isObjectGrippable(const AZ::EntityId entityId)
- {
- bool isGrippable = false;
- LmbrCentral::TagComponentRequestBus::EventResult(isGrippable, entityId, &LmbrCentral::TagComponentRequests::HasTag, GrippableTag);
- return isGrippable;
- }
- bool VacuumGripperComponent::TryToGripObject()
- {
- AZ_Warning(
- "VacuumGripper",
- m_gripperEffectorBodyHandle != AzPhysics::InvalidSimulatedBodyHandle,
- "Invalid body handle for gripperEffectorBody");
- if (m_gripperEffectorBodyHandle == AzPhysics::InvalidSimulatedBodyHandle)
- {
- // No articulation link found
- return true;
- }
- if (m_vacuumJoint != AzPhysics::InvalidJointHandle)
- {
- // Object is already gripped
- return true;
- }
- if (!m_grippedObjectInEffector.IsValid())
- {
- // No object to grip
- return false;
- }
- AZ_Assert(m_entity->FindComponent<PhysX::ArticulationLinkComponent>(), "No PhysX::ArticulationLinkComponent found on entity ");
- auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
- AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
- AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
- // Get gripped rigid body
- AzPhysics::RigidBody* grippedRigidBody = nullptr;
- Physics::RigidBodyRequestBus::EventResult(grippedRigidBody, m_grippedObjectInEffector, &Physics::RigidBodyRequests::GetRigidBody);
- AZ_Assert(grippedRigidBody, "No RigidBody found on entity grippedRigidBody");
- // Gripper is the end of the articulation chain
- AzPhysics::SimulatedBody* gripperBody = nullptr;
- gripperBody = sceneInterface->GetSimulatedBodyFromHandle(defaultSceneHandle, m_gripperEffectorBodyHandle);
- AZ_Assert(gripperBody, "No gripper body found");
- AttachToGripper(gripperBody, grippedRigidBody, sceneInterface);
- return true;
- }
- void VacuumGripperComponent::AttachToGripper(
- AzPhysics::SimulatedBody* gripperBody, AzPhysics::RigidBody* grippedRigidBody, AzPhysics::SceneInterface* sceneInterface)
- {
- AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
- AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
- // Find Transform of the child in parent's frame
- AZ::Transform childTransformWorld = grippedRigidBody->GetTransform();
- AZ::Transform parentsTranformWorld = gripperBody->GetTransform();
- AZ::Transform childTransformParent = parentsTranformWorld.GetInverse() * childTransformWorld;
- childTransformParent.Invert();
- // Configure new joint
- PhysX::FixedJointConfiguration jointConfig;
- jointConfig.m_debugName = "VacuumJoint";
- jointConfig.m_parentLocalRotation = AZ::Quaternion::CreateIdentity();
- jointConfig.m_parentLocalPosition = AZ::Vector3::CreateZero();
- jointConfig.m_childLocalRotation = childTransformParent.GetRotation();
- jointConfig.m_childLocalPosition = childTransformParent.GetTranslation();
- jointConfig.m_startSimulationEnabled = true;
- // Create new joint
- m_vacuumJoint =
- sceneInterface->AddJoint(defaultSceneHandle, &jointConfig, m_gripperEffectorBodyHandle, grippedRigidBody->m_bodyHandle);
- }
- void VacuumGripperComponent::ReleaseGrippedObject()
- {
- m_tryingToGrip = false;
- if (m_vacuumJoint == AzPhysics::InvalidJointHandle)
- {
- return;
- }
- auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
- AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
- AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
- // Wake up the body to prevent it from not moving after release
- AzPhysics::RigidBody* grippedRigidBody = nullptr;
- Physics::RigidBodyRequestBus::EventResult(grippedRigidBody, m_grippedObjectInEffector, &Physics::RigidBodyRequests::GetRigidBody);
- if (grippedRigidBody)
- {
- grippedRigidBody->ForceAwake();
- }
- sceneInterface->RemoveJoint(defaultSceneHandle, m_vacuumJoint);
- m_vacuumJoint = AzPhysics::InvalidJointHandle;
- }
- void VacuumGripperComponent::OnImGuiUpdate()
- {
- ImGui::Begin("VacuumGripperDebugger");
- AZStd::string grippedObjectInEffectorName;
- AZ::ComponentApplicationBus::BroadcastResult(
- grippedObjectInEffectorName, &AZ::ComponentApplicationRequests::GetEntityName, m_grippedObjectInEffector);
- ImGui::Text("Grippable object : %s", grippedObjectInEffectorName.c_str());
- ImGui::Text("Vacuum joint created : %d ", m_vacuumJoint != AzPhysics::InvalidJointHandle);
- ImGui::Checkbox("Gripping", &m_tryingToGrip);
- if (ImGui::Button("Grip Command "))
- {
- m_tryingToGrip = true;
- }
- if (ImGui::Button("Release Command"))
- {
- ReleaseGrippedObject();
- }
- ImGui::End();
- }
- AZ::Outcome<void, AZStd::string> VacuumGripperComponent::GripperCommand(float position, float maxEffort)
- {
- AZ_Trace("VacuumGripper", "GripperCommand %f\n", position);
- m_cancelGripperCommand = false;
- if (position == 0.0f)
- {
- m_tryingToGrip = true;
- }
- else
- {
- ReleaseGrippedObject();
- }
- return AZ::Success();
- }
- AZ::Outcome<void, AZStd::string> VacuumGripperComponent::CancelGripperCommand()
- {
- ReleaseGrippedObject();
- m_cancelGripperCommand = true;
- return AZ::Success();
- }
- bool VacuumGripperComponent::HasGripperCommandBeenCancelled() const
- {
- if (m_cancelGripperCommand && m_vacuumJoint == AzPhysics::InvalidJointHandle)
- {
- return true;
- }
- return false;
- }
- float VacuumGripperComponent::GetGripperPosition() const
- {
- return m_tryingToGrip ? 0.0f : 1.0f;
- }
- float VacuumGripperComponent::GetGripperEffort() const
- {
- return m_vacuumJoint == AzPhysics::InvalidJointHandle ? 0.0f : 1.0f;
- }
- bool VacuumGripperComponent::IsGripperNotMoving() const
- {
- return true;
- }
- bool VacuumGripperComponent::HasGripperReachedGoal() const
- {
- const bool isObjectAttached = (m_vacuumJoint != AzPhysics::InvalidJointHandle);
- if (m_tryingToGrip && isObjectAttached)
- {
- return true;
- }
- if (!m_tryingToGrip && !isObjectAttached)
- {
- return true;
- }
- return false;
- }
- } // namespace ROS2
|