Some changes to motorized joint (#272)

Corrected world rotation
Expose more headers

Signed-off-by: Michał Pełka <[email protected]>

Gems/ROS2/Code/Include/ROS2/Manipulator/MotorizedJoint.h

@@ -7,7 +7,7 @@
  */
 #pragma once
 
-#include "VehicleDynamics/DriveModels/PidConfiguration.h"
+#include "ROS2/VehicleDynamics/DriveModels/PidConfiguration.h"
 #include <AzCore/Component/Component.h>
 #include <AzCore/Component/TickBus.h>
 
@@ -49,6 +49,12 @@ namespace ROS2
         //! @returns current position  in meters for linear joints and radians for angular
         float GetCurrentPosition() const;
 
+        //! Get a degree of freedom direction.
+        AZ::Vector3 GetDir() const
+        {
+            return m_jointDir;
+        };
+
     private:
         float ComputeMeasurement(AZ::ScriptTimePoint time);
 

Gems/ROS2/Code/Source/Manipulator/MotorizedJoint.cpp

@@ -133,11 +133,12 @@ namespace ROS2
         {
             AZ_Printf(
                 "MotorizedJoint",
-                " %s | pos: %f | err: %f | cntrl : %f |",
+                " %s | pos: %f | err: %f | cntrl : %f | set : %f |",
                 GetEntity()->GetName().c_str(),
                 measurement,
                 control_position_error,
-                speed_control);
+                speed_control,
+                m_setpoint);
         }
         SetVelocity(speed_control, deltaTime);
     }
@@ -189,17 +190,18 @@ namespace ROS2
 
     void MotorizedJoint::ApplyLinVelRigidBodyImpulse(float velocity, float deltaTime)
     {
-        AZ::Transform transform;
-        AZ::TransformBus::EventResult(transform, this->GetEntityId(), &AZ::TransformBus::Events::GetLocalTM);
+        AZ::Quaternion transform;
+        AZ::TransformBus::EventResult(transform, this->GetEntityId(), &AZ::TransformBus::Events::GetWorldRotationQuaternion);
         auto force_impulse = transform.TransformVector(m_jointDir * velocity);
+        AZ_Printf("ApplyLinVelRigidBodyImpulse", "%f %f %f ", force_impulse.GetX(), force_impulse.GetY(), force_impulse.GetZ());
         Physics::RigidBodyRequestBus::Event(
             this->GetEntityId(), &Physics::RigidBodyRequests::ApplyLinearImpulse, force_impulse * deltaTime);
     }
 
     void MotorizedJoint::ApplyLinVelRigidBody(float velocity, float deltaTime)
     {
-        AZ::Transform transform;
-        AZ::TransformBus::EventResult(transform, this->GetEntityId(), &AZ::TransformBus::Events::GetLocalTM);
+        AZ::Quaternion transform;
+        AZ::TransformBus::EventResult(transform, this->GetEntityId(), &AZ::TransformBus::Events::GetWorldRotationQuaternion);
         AZ::Vector3 currentVelocity;
         auto transformed_velocity_increment = transform.TransformVector(m_jointDir * velocity);
         Physics::RigidBodyRequestBus::EventResult(currentVelocity, this->GetEntityId(), &Physics::RigidBodyRequests::GetLinearVelocity);

Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/AckermannDriveModel.h

@@ -7,8 +7,8 @@
  */
 #pragma once
 
+#include "ROS2/VehicleDynamics/DriveModels/PidConfiguration.h"
 #include "VehicleDynamics/DriveModel.h"
-#include "VehicleDynamics/DriveModels/PidConfiguration.h"
 #include "VehicleDynamics/VehicleConfiguration.h"
 #include "VehicleDynamics/VehicleInputsState.h"
 #include "VehicleDynamics/WheelDynamicsData.h"

Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/PidConfiguration.cpp

@@ -6,7 +6,7 @@
  *
  */
 
-#include "VehicleDynamics/DriveModels/PidConfiguration.h"
+#include "ROS2/VehicleDynamics/DriveModels/PidConfiguration.h"
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
 #include <AzCore/Serialization/SerializeContext.h>

Gems/ROS2/Code/ros2_files.cmake

@@ -72,7 +72,6 @@ set(FILES
         Source/Converters/URDF/ToFBX/UrdfToFbxConverter.cpp
         Source/Converters/URDF/ToFBX/UrdfToFbxConverter.h
         Source/VehicleDynamics/DriveModels/PidConfiguration.cpp
-        Source/VehicleDynamics/DriveModels/PidConfiguration.h
         Source/VehicleDynamics/DriveModels/AckermannDriveModel.cpp
         Source/VehicleDynamics/DriveModels/AckermannDriveModel.h
         Source/VehicleDynamics/AxleConfiguration.cpp

Gems/ROS2/Code/ros2_header_files.cmake

@@ -12,4 +12,5 @@ set(FILES
         Include/ROS2/Manipulator/MotorizedJoint.h
         Include/ROS2/Utilities/ROS2Conversions.h
         Include/ROS2/Utilities/ROS2Names.h
+        Include/ROS2/VehicleDynamics/DriveModels/PidConfiguration.h
         )