add actuator position update during communication (#236)

WPI-AIM · Aug 15, 2024 · 09a28ff · 09a28ff
1 parent accb6ba
commit 09a28ff
Showing 1 changed file with 7 additions and 0 deletions.
diff --git 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++;