From 09a28ff00b34c71f7ad798c5deda59b3a8e851bb Mon Sep 17 00:00:00 2001
From: "Shiyue(Vanessa) Wang" <44687580+capricieuxV@users.noreply.github.com>
Date: Thu, 15 Aug 2024 12:24:05 -0400
Subject: [PATCH] add actuator position update during communication (#236)

---
 ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp b/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp
index 5a482dec..73c97097 100644
--- a/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp
+++ b/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp
@@ -429,6 +429,13 @@ void afObjectCommunicationPlugin::actuatorUpdateState(afActuatorPtr actPtr, doub
     m_actuatorCommPtr->m_writeMtx.lock();
     setTimeStamps(m_objectPtr->m_afWorld->getWallTime(), m_objectPtr->m_afWorld->getSimulationTime(), m_objectPtr->getCurrentTimeStamp());
     m_actuatorCommPtr->set_parent_name(actPtr->m_parentName);
+
+    cVector3d localPos = actPtr->getLocalPos();
+    m_actuatorCommPtr->cur_position(localPos.x(), localPos.y(), localPos.z());
+    cQuaternion q;
+    q.fromRotMat(actPtr->getLocalRot());
+    m_actuatorCommPtr->cur_orientation(q.x, q.y, q.z, q.w);
+
     m_actuatorCommPtr->m_writeMtx.unlock();
     m_actuatorCommPtr->enableComm();
     m_write_count++;