VacuumGripperComponent.cpp 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342
  1. /*
  2. * Copyright (c) Contributors to the Open 3D Engine Project.
  3. * For complete copyright and license terms please see the LICENSE at the root
  4. * of this distribution.
  5. *
  6. * SPDX-License-Identifier: Apache-2.0 OR MIT
  7. *
  8. */
  9. #include "VacuumGripperComponent.h"
  10. #include "Source/ArticulationLinkComponent.h"
  11. #include "Utils.h"
  12. #include <Utilities/ArticulationsUtilities.h>
  13. #include <AzCore/Serialization/EditContext.h>
  14. #include <AzCore/Serialization/SerializeContext.h>
  15. #include <AzFramework/Components/TransformComponent.h>
  16. #include <AzFramework/Physics/PhysicsSystem.h>
  17. #include <AzFramework/Physics/RigidBodyBus.h>
  18. #include <AzFramework/Physics/SimulatedBodies/RigidBody.h>
  19. #include <imgui/imgui.h>
  20. namespace ROS2
  21. {
  22. void VacuumGripperComponent::Activate()
  23. {
  24. m_gripperEffectorBodyHandle = AzPhysics::InvalidSimulatedBodyHandle;
  25. m_onTriggerEnterHandler = AzPhysics::SimulatedBodyEvents::OnTriggerEnter::Handler(
  26. [&]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, [[maybe_unused]] const AzPhysics::TriggerEvent& event)
  27. {
  28. const auto grippedEntityCandidateId = event.m_otherBody->GetEntityId();
  29. const bool isGrippable = isObjectGrippable(grippedEntityCandidateId);
  30. if (isGrippable)
  31. {
  32. m_grippedObjectInEffector = grippedEntityCandidateId;
  33. }
  34. });
  35. m_onTriggerExitHandler = AzPhysics::SimulatedBodyEvents::OnTriggerExit::Handler(
  36. [&]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, [[maybe_unused]] const AzPhysics::TriggerEvent& event)
  37. {
  38. const auto grippedEntityCandidateId = event.m_otherBody->GetEntityId();
  39. if (m_grippedObjectInEffector == grippedEntityCandidateId)
  40. {
  41. m_grippedObjectInEffector = AZ::EntityId(AZ::EntityId::InvalidEntityId);
  42. }
  43. });
  44. m_vacuumJoint = AzPhysics::InvalidJointHandle;
  45. m_grippedObjectInEffector = AZ::EntityId(AZ::EntityId::InvalidEntityId);
  46. m_tryingToGrip = false;
  47. m_cancelGripperCommand = false;
  48. AZ::TickBus::Handler::BusConnect();
  49. ImGui::ImGuiUpdateListenerBus::Handler::BusConnect();
  50. GripperRequestBus::Handler::BusConnect(GetEntityId());
  51. }
  52. void VacuumGripperComponent::Deactivate()
  53. {
  54. m_onTriggerEnterHandler.Disconnect();
  55. m_onTriggerExitHandler.Disconnect();
  56. GripperRequestBus::Handler::BusDisconnect();
  57. AZ::TickBus::Handler::BusDisconnect();
  58. ImGui::ImGuiUpdateListenerBus::Handler::BusDisconnect();
  59. }
  60. void VacuumGripperComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
  61. {
  62. provided.push_back(AZ_CRC_CE("GripperService"));
  63. }
  64. void VacuumGripperComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
  65. {
  66. incompatible.push_back(AZ_CRC_CE("GripperService"));
  67. }
  68. void VacuumGripperComponent::Reflect(AZ::ReflectContext* context)
  69. {
  70. if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
  71. {
  72. serialize->Class<VacuumGripperComponent, AZ::Component>()
  73. ->Field("EffectorCollider", &VacuumGripperComponent::m_gripperEffectorCollider)
  74. ->Field("EffectorArticulation", &VacuumGripperComponent::m_gripperEffectorArticulationLink)
  75. ->Version(1);
  76. if (AZ::EditContext* ec = serialize->GetEditContext())
  77. {
  78. ec->Class<VacuumGripperComponent>("VacuumGripperComponent", "Component for control of a vacuum gripper.")
  79. ->ClassElement(AZ::Edit::ClassElements::EditorData, "VacuumGripper")
  80. ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
  81. ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
  82. ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/VacuumGripperComponent.svg")
  83. ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/VacuumGripperComponent.svg")
  84. ->DataElement(
  85. AZ::Edit::UIHandlers::EntityId,
  86. &VacuumGripperComponent::m_gripperEffectorCollider,
  87. "Effector Trigger Collider",
  88. "The entity with trigger collider that will detect objects that can be successfully gripped.")
  89. ->DataElement(
  90. AZ::Edit::UIHandlers::EntityId,
  91. &VacuumGripperComponent::m_gripperEffectorArticulationLink,
  92. "Effector Articulation Link",
  93. "The entity that is the articulation link of the effector.");
  94. }
  95. }
  96. }
  97. void VacuumGripperComponent::OnTick(float delta, AZ::ScriptTimePoint timePoint)
  98. {
  99. AZ_Assert(AZ::Interface<AzPhysics::SystemInterface>::Get(), "No physics system.");
  100. AzPhysics::SceneInterface* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
  101. AZ_Assert(sceneInterface, "No scene intreface.");
  102. AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
  103. AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle.");
  104. // Connect the trigger handlers if not already connected, it is circumventing the issue GH-16188, the
  105. // RigidbodyNotificationBus should be used instead.
  106. if (!m_onTriggerEnterHandler.IsConnected() || !m_onTriggerExitHandler.IsConnected())
  107. {
  108. if (auto* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get())
  109. {
  110. AZStd::pair<AzPhysics::SceneHandle, AzPhysics::SimulatedBodyHandle> foundBody =
  111. physicsSystem->FindAttachedBodyHandleFromEntityId(m_gripperEffectorCollider);
  112. AZ_Warning(
  113. "VacuumGripper", foundBody.first != AzPhysics::InvalidSceneHandle, "No body found for m_gripperEffectorCollider.");
  114. if (foundBody.first != AzPhysics::InvalidSceneHandle)
  115. {
  116. AzPhysics::SimulatedBodyEvents::RegisterOnTriggerEnterHandler(
  117. foundBody.first, foundBody.second, m_onTriggerEnterHandler);
  118. AzPhysics::SimulatedBodyEvents::RegisterOnTriggerExitHandler(foundBody.first, foundBody.second, m_onTriggerExitHandler);
  119. }
  120. }
  121. AZ::EntityId rootArticulationEntity = Utils::GetRootOfArticulation(m_gripperEffectorArticulationLink);
  122. AZ::Entity* rootEntity = nullptr;
  123. AZ::ComponentApplicationBus::BroadcastResult(rootEntity, &AZ::ComponentApplicationRequests::FindEntity, rootArticulationEntity);
  124. AZ_Trace("VacuumGripper", "Root articulation entity name: %s\n", rootEntity->GetName().c_str());
  125. PhysX::ArticulationLinkComponent* component = rootEntity->FindComponent<PhysX::ArticulationLinkComponent>();
  126. AZStd::vector<AzPhysics::SimulatedBodyHandle> articulationHandles = component->GetSimulatedBodyHandles();
  127. AZ_Assert(articulationHandles.size() > 1, "Expected more than one body handles in articulations");
  128. for (AzPhysics::SimulatedBodyHandle handle : articulationHandles)
  129. {
  130. AzPhysics::SimulatedBody* body = sceneInterface->GetSimulatedBodyFromHandle(defaultSceneHandle, handle);
  131. AZ_Assert(body, "Expected valid body pointer");
  132. if (body->GetEntityId() == m_gripperEffectorArticulationLink)
  133. {
  134. m_gripperEffectorBodyHandle = handle;
  135. }
  136. }
  137. }
  138. if (m_tryingToGrip)
  139. {
  140. TryToGripObject();
  141. }
  142. }
  143. bool VacuumGripperComponent::isObjectGrippable(const AZ::EntityId entityId)
  144. {
  145. bool isGrippable = false;
  146. LmbrCentral::TagComponentRequestBus::EventResult(isGrippable, entityId, &LmbrCentral::TagComponentRequests::HasTag, GrippableTag);
  147. return isGrippable;
  148. }
  149. bool VacuumGripperComponent::TryToGripObject()
  150. {
  151. AZ_Warning(
  152. "VacuumGripper",
  153. m_gripperEffectorBodyHandle != AzPhysics::InvalidSimulatedBodyHandle,
  154. "Invalid body handle for gripperEffectorBody");
  155. if (m_gripperEffectorBodyHandle == AzPhysics::InvalidSimulatedBodyHandle)
  156. {
  157. // No articulation link found
  158. return true;
  159. }
  160. if (m_vacuumJoint != AzPhysics::InvalidJointHandle)
  161. {
  162. // Object is already gripped
  163. return true;
  164. }
  165. if (!m_grippedObjectInEffector.IsValid())
  166. {
  167. // No object to grip
  168. return false;
  169. }
  170. AZ_Assert(m_entity->FindComponent<PhysX::ArticulationLinkComponent>(), "No PhysX::ArticulationLinkComponent found on entity ");
  171. auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
  172. AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
  173. AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
  174. // Get gripped rigid body
  175. AzPhysics::RigidBody* grippedRigidBody = nullptr;
  176. Physics::RigidBodyRequestBus::EventResult(grippedRigidBody, m_grippedObjectInEffector, &Physics::RigidBodyRequests::GetRigidBody);
  177. AZ_Assert(grippedRigidBody, "No RigidBody found on entity grippedRigidBody");
  178. // Gripper is the end of the articulation chain
  179. AzPhysics::SimulatedBody* gripperBody = nullptr;
  180. gripperBody = sceneInterface->GetSimulatedBodyFromHandle(defaultSceneHandle, m_gripperEffectorBodyHandle);
  181. AZ_Assert(gripperBody, "No gripper body found");
  182. AttachToGripper(gripperBody, grippedRigidBody, sceneInterface);
  183. return true;
  184. }
  185. void VacuumGripperComponent::AttachToGripper(
  186. AzPhysics::SimulatedBody* gripperBody, AzPhysics::RigidBody* grippedRigidBody, AzPhysics::SceneInterface* sceneInterface)
  187. {
  188. AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
  189. AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
  190. // Find Transform of the child in parent's frame
  191. AZ::Transform childTransformWorld = grippedRigidBody->GetTransform();
  192. AZ::Transform parentsTranformWorld = gripperBody->GetTransform();
  193. AZ::Transform childTransformParent = parentsTranformWorld.GetInverse() * childTransformWorld;
  194. childTransformParent.Invert();
  195. // Configure new joint
  196. PhysX::FixedJointConfiguration jointConfig;
  197. jointConfig.m_debugName = "VacuumJoint";
  198. jointConfig.m_parentLocalRotation = AZ::Quaternion::CreateIdentity();
  199. jointConfig.m_parentLocalPosition = AZ::Vector3::CreateZero();
  200. jointConfig.m_childLocalRotation = childTransformParent.GetRotation();
  201. jointConfig.m_childLocalPosition = childTransformParent.GetTranslation();
  202. jointConfig.m_startSimulationEnabled = true;
  203. // Create new joint
  204. m_vacuumJoint =
  205. sceneInterface->AddJoint(defaultSceneHandle, &jointConfig, m_gripperEffectorBodyHandle, grippedRigidBody->m_bodyHandle);
  206. }
  207. void VacuumGripperComponent::ReleaseGrippedObject()
  208. {
  209. m_tryingToGrip = false;
  210. if (m_vacuumJoint == AzPhysics::InvalidJointHandle)
  211. {
  212. return;
  213. }
  214. auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
  215. AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
  216. AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
  217. // Wake up the body to prevent it from not moving after release
  218. AzPhysics::RigidBody* grippedRigidBody = nullptr;
  219. Physics::RigidBodyRequestBus::EventResult(grippedRigidBody, m_grippedObjectInEffector, &Physics::RigidBodyRequests::GetRigidBody);
  220. if (grippedRigidBody)
  221. {
  222. grippedRigidBody->ForceAwake();
  223. }
  224. sceneInterface->RemoveJoint(defaultSceneHandle, m_vacuumJoint);
  225. m_vacuumJoint = AzPhysics::InvalidJointHandle;
  226. }
  227. void VacuumGripperComponent::OnImGuiUpdate()
  228. {
  229. ImGui::Begin("VacuumGripperDebugger");
  230. AZStd::string grippedObjectInEffectorName;
  231. AZ::ComponentApplicationBus::BroadcastResult(
  232. grippedObjectInEffectorName, &AZ::ComponentApplicationRequests::GetEntityName, m_grippedObjectInEffector);
  233. ImGui::Text("Grippable object : %s", grippedObjectInEffectorName.c_str());
  234. ImGui::Text("Vacuum joint created : %d ", m_vacuumJoint != AzPhysics::InvalidJointHandle);
  235. ImGui::Checkbox("Gripping", &m_tryingToGrip);
  236. if (ImGui::Button("Grip Command "))
  237. {
  238. m_tryingToGrip = true;
  239. }
  240. if (ImGui::Button("Release Command"))
  241. {
  242. ReleaseGrippedObject();
  243. }
  244. ImGui::End();
  245. }
  246. AZ::Outcome<void, AZStd::string> VacuumGripperComponent::GripperCommand(float position, float maxEffort)
  247. {
  248. AZ_Trace("VacuumGripper", "GripperCommand %f\n", position);
  249. m_cancelGripperCommand = false;
  250. if (position == 0.0f)
  251. {
  252. m_tryingToGrip = true;
  253. }
  254. else
  255. {
  256. ReleaseGrippedObject();
  257. }
  258. return AZ::Success();
  259. }
  260. AZ::Outcome<void, AZStd::string> VacuumGripperComponent::CancelGripperCommand()
  261. {
  262. ReleaseGrippedObject();
  263. m_cancelGripperCommand = true;
  264. return AZ::Success();
  265. }
  266. bool VacuumGripperComponent::HasGripperCommandBeenCancelled() const
  267. {
  268. if (m_cancelGripperCommand && m_vacuumJoint == AzPhysics::InvalidJointHandle)
  269. {
  270. return true;
  271. }
  272. return false;
  273. }
  274. float VacuumGripperComponent::GetGripperPosition() const
  275. {
  276. return m_tryingToGrip ? 0.0f : 1.0f;
  277. }
  278. float VacuumGripperComponent::GetGripperEffort() const
  279. {
  280. return m_vacuumJoint == AzPhysics::InvalidJointHandle ? 0.0f : 1.0f;
  281. }
  282. bool VacuumGripperComponent::IsGripperNotMoving() const
  283. {
  284. return true;
  285. }
  286. bool VacuumGripperComponent::HasGripperReachedGoal() const
  287. {
  288. const bool isObjectAttached = (m_vacuumJoint != AzPhysics::InvalidJointHandle);
  289. if (m_tryingToGrip && isObjectAttached)
  290. {
  291. return true;
  292. }
  293. if (!m_tryingToGrip && !isObjectAttached)
  294. {
  295. return true;
  296. }
  297. return false;
  298. }
  299. } // namespace ROS2