Filled the published JointState message (#388)

Added getters for velocity and effort in JointsManipulationRequests.h. Added this info to the published JointState message

Signed-off-by: Antoni Puch <[email protected]>

Gems/ROS2/Code/Include/ROS2/Manipulation/JointInfo.h

@@ -17,6 +17,8 @@
 namespace ROS2
 {
     using JointPosition = float;
+    using JointVelocity = float;
+    using JointEffort = float;
     struct JointInfo
     {
         AZ_TYPE_INFO(JointInfo, "{2E33E4D0-78DD-436D-B3AB-F752E744F421}");

Gems/ROS2/Code/Include/ROS2/Manipulation/JointsManipulationRequests.h

@@ -24,6 +24,8 @@ namespace ROS2
         static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
 
         using JointsPositionsMap = AZStd::unordered_map<AZStd::string, JointPosition>;
+        using JointsVelocitiesMap = AZStd::unordered_map<AZStd::string, JointVelocity>;
+        using JointsEffortsMap = AZStd::unordered_map<AZStd::string, JointEffort>;
 
         //! Get all entity tree joints, including joint or articulation component hierarchy.
         //! @return An unordered map of joint names to joint info structure.
@@ -37,10 +39,32 @@ namespace ROS2
         //! If it does not exist or some other error happened, error message is returned.
         virtual AZ::Outcome<JointPosition, AZStd::string> GetJointPosition(const AZStd::string& jointName) = 0;
 
+        //! Get velocity of a joint by name.
+        //! Works with hinge joints and articulation links.
+        //! @param jointName name of the joint. Use names acquired from GetJoints() query.
+        //! @return outcome with velocity if joint exists.
+        //! If it does not exist or some other error happened, error message is returned.
+        virtual AZ::Outcome<JointVelocity, AZStd::string> GetJointVelocity(const AZStd::string& jointName) = 0;
+
         //! Return positions of all single DOF joints.
         //! @return a vector of all joints relative positions in degree of motion range or error message.
         virtual JointsPositionsMap GetAllJointsPositions() = 0;
 
+        //! Return velocities of all single DOF joints.
+        //! @return a vector of all joints velocities or error message.
+        virtual JointsVelocitiesMap GetAllJointsVelocities() = 0;
+
+        //! Get effort of a force-driven articulation link by name.
+        //! If the joint is not an articulation link, or it's acceleration-driven, returns 0.
+        //! @param jointName name of the joint. Use names acquired from GetJoints() query.
+        //! @return outcome with effort if joint exists.
+        //! If it does not exist or some other error happened, error message is returned.
+        virtual AZ::Outcome<JointEffort, AZStd::string> GetJointEffort(const AZStd::string& jointName) = 0;
+
+        //! Return efforts of all single DOF joints.
+        //! @return a vector of all joints efforts or error message.
+        virtual JointsEffortsMap GetAllJointsEfforts() = 0;
+
         //! Move specified joints into positions.
         //! @param new positions for each named joint. Use names queried through GetJoints().
         //! @return nothing on success, error message on failure.

Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.cpp

@@ -41,13 +41,19 @@ namespace ROS2
         for (const auto& [jointName, jointInfo] : manipulatorJoints)
         {
             AZ::Outcome<float, AZStd::string> result;
-            JointsManipulationRequestBus::EventResult(result, m_context.m_entityId, &JointsManipulationRequests::GetJointPosition, jointName);
+            JointsManipulationRequestBus::EventResult(
+                result, m_context.m_entityId, &JointsManipulationRequests::GetJointPosition, jointName);
             auto currentJointPosition = result.GetValue();
+            JointsManipulationRequestBus::EventResult(
+                result, m_context.m_entityId, &JointsManipulationRequests::GetJointVelocity, jointName);
+            auto currentJointVelocity = result.GetValue();
+            JointsManipulationRequestBus::EventResult(result, m_context.m_entityId, &JointsManipulationRequests::GetJointEffort, jointName);
+            auto currentJointEffort = result.GetValue();
 
             m_jointStateMsg.name[i] = jointName.c_str();
             m_jointStateMsg.position[i] = currentJointPosition;
-            m_jointStateMsg.velocity[i] = 0.0;
-            m_jointStateMsg.effort[i] = 0.0;
+            m_jointStateMsg.velocity[i] = currentJointVelocity;
+            m_jointStateMsg.effort[i] = currentJointEffort;
             i++;
         }
         m_jointStatePublisher->publish(m_jointStateMsg);

Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.cpp

@@ -222,6 +222,30 @@ namespace ROS2
         return AZ::Success(position);
     }
 
+    AZ::Outcome<JointVelocity, AZStd::string> JointsManipulationComponent::GetJointVelocity(const AZStd::string& jointName)
+    {
+        if (!m_manipulationJoints.contains(jointName))
+        {
+            return AZ::Failure(AZStd::string::format("Joint %s does not exist", jointName.c_str()));
+        }
+
+        auto jointInfo = m_manipulationJoints.at(jointName);
+        float velocity{ 0 };
+        if (jointInfo.m_isArticulation)
+        {
+            PhysX::ArticulationJointRequestBus::EventResult(
+                velocity,
+                jointInfo.m_entityComponentIdPair.GetEntityId(),
+                &PhysX::ArticulationJointRequests::GetJointVelocity,
+                jointInfo.m_axis);
+        }
+        else
+        {
+            PhysX::JointRequestBus::EventResult(velocity, jointInfo.m_entityComponentIdPair, &PhysX::JointRequests::GetVelocity);
+        }
+        return AZ::Success(velocity);
+    }
+
     JointsManipulationRequests::JointsPositionsMap JointsManipulationComponent::GetAllJointsPositions()
     {
         JointsManipulationRequests::JointsPositionsMap positions;
@@ -232,6 +256,77 @@ namespace ROS2
         return positions;
     }
 
+    JointsManipulationRequests::JointsVelocitiesMap JointsManipulationComponent::GetAllJointsVelocities()
+    {
+        JointsManipulationRequests::JointsVelocitiesMap velocities;
+        for (const auto& [jointName, jointInfo] : m_manipulationJoints)
+        {
+            velocities[jointName] = GetJointVelocity(jointName).GetValue();
+        }
+        return velocities;
+    }
+
+    AZ::Outcome<JointEffort, AZStd::string> JointsManipulationComponent::GetJointEffort(const AZStd::string& jointName)
+    {
+        if (!m_manipulationJoints.contains(jointName))
+        {
+            return AZ::Failure(AZStd::string::format("Joint %s does not exist", jointName.c_str()));
+        }
+
+        auto jointInfo = m_manipulationJoints.at(jointName);
+        float effort{ 0 };
+
+        if (!jointInfo.m_isArticulation)
+        {
+            // The joint isn't controlled by JointsArticulationControllerComponent.
+            // There is no easy way to calculate the effort.
+            return AZ::Success(effort);
+        }
+
+        bool is_acceleration_driven{ false };
+        PhysX::ArticulationJointRequestBus::EventResult(
+            is_acceleration_driven,
+            jointInfo.m_entityComponentIdPair.GetEntityId(),
+            &PhysX::ArticulationJointRequests::IsAccelerationDrive,
+            jointInfo.m_axis);
+
+        if (is_acceleration_driven)
+        {
+            // The formula below in this case will calculate acceleration, not effort.
+            // In this case we can't calculate the effort without PhysX engine support.
+            return AZ::Success(effort);
+        }
+
+        float stiffness{ 0 }, damping{ 0 }, target_pos{ 0 }, position{ 0 }, target_vel{ 0 }, velocity{ 0 }, max_force{ 0 };
+        PhysX::ArticulationJointRequestBus::Event(
+            jointInfo.m_entityComponentIdPair.GetEntityId(),
+            [&](PhysX::ArticulationJointRequests* articulationJointRequests)
+            {
+                stiffness = articulationJointRequests->GetDriveStiffness(jointInfo.m_axis);
+                damping = articulationJointRequests->GetDriveDamping(jointInfo.m_axis);
+                target_pos = articulationJointRequests->GetDriveTarget(jointInfo.m_axis);
+                position = articulationJointRequests->GetJointPosition(jointInfo.m_axis);
+                target_vel = articulationJointRequests->GetDriveTargetVelocity(jointInfo.m_axis);
+                velocity = articulationJointRequests->GetJointVelocity(jointInfo.m_axis);
+                max_force = articulationJointRequests->GetMaxForce(jointInfo.m_axis);
+            });
+        effort = stiffness * -(position - target_pos) + damping * (target_vel - velocity);
+
+        effort = AZ::GetClamp(effort, -max_force, max_force);
+
+        return AZ::Success(effort);
+    }
+
+    JointsManipulationRequests::JointsEffortsMap JointsManipulationComponent::GetAllJointsEfforts()
+    {
+        JointsManipulationRequests::JointsEffortsMap efforts;
+        for (const auto& [jointName, jointInfo] : m_manipulationJoints)
+        {
+            efforts[jointName] = GetJointEffort(jointName).GetValue();
+        }
+        return efforts;
+    }
+
     AZ::Outcome<void, AZStd::string> JointsManipulationComponent::MoveJointToPosition(
         const AZStd::string& jointName, JointPosition position)
     {

Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.h

@@ -42,8 +42,16 @@ namespace ROS2
         ManipulationJoints GetJoints() override;
         //! @see ROS2::JointsManipulationRequestBus::GetJointPosition
         AZ::Outcome<JointPosition, AZStd::string> GetJointPosition(const AZStd::string& jointName) override;
+        //! @see ROS2::JointsManipulationRequestBus::GetJointVelocity
+        AZ::Outcome<JointVelocity, AZStd::string> GetJointVelocity(const AZStd::string& jointName) override;
         //! @see ROS2::JointsManipulationRequestBus::GetAllJointsPositions
         JointsPositionsMap GetAllJointsPositions() override;
+        //! @see ROS2::JointsManipulationRequestBus::GetAllJointsVelocities
+        JointsVelocitiesMap GetAllJointsVelocities() override;
+        //! @see ROS2::JointsManipulationRequestBus::GetJointEffort
+        AZ::Outcome<JointEffort, AZStd::string> GetJointEffort(const AZStd::string& jointName) override;
+        //! @see ROS2::JointsManipulationRequestBus::GetAllJointsEfforts
+        JointsEffortsMap GetAllJointsEfforts() override;
         //! @see ROS2::JointsManipulationRequestBus::MoveJointsToPositions
         AZ::Outcome<void, AZStd::string> MoveJointsToPositions(const JointsPositionsMap& positions) override;
         //! @see ROS2::JointsManipulationRequestBus::MoveJointToPosition