InertialsMaker.cpp 2.7 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859
  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 of this distribution.
  4. *
  5. * SPDX-License-Identifier: Apache-2.0 OR MIT
  6. *
  7. */
  8. #include "InertialsMaker.h"
  9. #include <AzCore/Component/EntityId.h>
  10. #include <AzToolsFramework/Entity/EditorEntityHelpers.h>
  11. #include <RobotImporter/Utils/TypeConversions.h>
  12. #include <Source/EditorRigidBodyComponent.h>
  13. namespace ROS2
  14. {
  15. // Here is the recommended, minimal number of iterations for position and velocity solver.
  16. // It is needed since currently o3de default values are optimized for the gaming experience, not a simulation.
  17. constexpr AZ::u8 kMinimalNumPosSolv = 40;
  18. constexpr AZ::u8 kMinimalNumVelSolv = 10;
  19. void InertialsMaker::AddInertial(urdf::InertialSharedPtr inertial, AZ::EntityId entityId) const
  20. {
  21. if (!inertial)
  22. { // it is ok not to have inertia in a link
  23. return;
  24. }
  25. AZ_TracePrintf("AddInertial", "Processing inertial for entity id: %s\n", entityId.ToString().c_str());
  26. AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId);
  27. PhysX::EditorRigidBodyConfiguration rigidBodyConfiguration;
  28. PhysX::RigidBodyConfiguration physxSpecificConfiguration;
  29. physxSpecificConfiguration.m_solverPositionIterations =
  30. AZStd::max(physxSpecificConfiguration.m_solverPositionIterations, kMinimalNumPosSolv);
  31. physxSpecificConfiguration.m_solverVelocityIterations =
  32. AZStd::max(physxSpecificConfiguration.m_solverVelocityIterations, kMinimalNumVelSolv);
  33. rigidBodyConfiguration.m_mass = inertial->mass;
  34. rigidBodyConfiguration.m_computeMass = false;
  35. rigidBodyConfiguration.m_centerOfMassOffset = URDF::TypeConversions::ConvertVector3(inertial->origin.position);
  36. rigidBodyConfiguration.m_computeCenterOfMass = false;
  37. if (!URDF::TypeConversions::ConvertQuaternion(inertial->origin.rotation).IsIdentity())
  38. { // There is a rotation component in URDF that we are not able to apply
  39. AZ_Warning("AddInertial", false, "Ignoring URDF inertial origin rotation (no such field in rigid body configuration)");
  40. }
  41. // Inertia tensor is symmetrical
  42. auto inertiaMatrix = AZ::Matrix3x3::CreateFromRows(
  43. AZ::Vector3(inertial->ixx, inertial->ixy, inertial->ixz),
  44. AZ::Vector3(inertial->ixy, inertial->iyy, inertial->iyz),
  45. AZ::Vector3(inertial->ixz, inertial->iyz, inertial->izz));
  46. rigidBodyConfiguration.m_inertiaTensor = inertiaMatrix;
  47. rigidBodyConfiguration.m_computeInertiaTensor = false;
  48. entity->CreateComponent<PhysX::EditorRigidBodyComponent>(rigidBodyConfiguration, physxSpecificConfiguration);
  49. }
  50. } // namespace ROS2