Issue WPI-AIM#163. Setting activeControllerType to Force.

adnanmunawar · Apr 9, 2022 · 1155775 · 1155775
1 parent b444cc5
commit 1155775
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 2 deletions.
diff --git a/ambf_framework/afInputDevices.cpp b/ambf_framework/afInputDevices.cpp
@@ -640,9 +640,13 @@ bool afSimulatedDevice::createFromAttribs(afSimulatedDeviceAttribs *a_attribs)
         // Since an existing root body is bound to the physical device whose afComm should already be
         // running
         if(attribs.m_sdeDefined){
-            std::string simDevName = "simulated_device_" + std::to_string(m_phyDev->m_CCU_Manager->s_inputDeviceCount) + modelName;
+            m_rootLink->setPassive(false);
+            m_rootLink->setNamespace(m_rootLink->getNamespace() + "/simulated_device/");
             m_rootLink->loadCommunicationPlugin(m_rootLink, a_attribs);
         }
+
+        // Initialize the default controller to be the force controller
+        m_rootLink->m_activeControllerType = afControlType::FORCE;
     }
     else{
         cerr << "ERROR! FAILED TO LOAD ROOT LINK FOR MODEL " << attribs.m_modelAttribs.m_filePath.c_str() << endl;

diff --git a/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp b/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp
@@ -555,7 +555,7 @@ void afObjectCommunicationPlugin::rigidBodyFetchCommand(afRigidBodyPtr afRBPtr,
     // IF THE COMMAND IS OF TYPE FORCE
     switch (afCommand.cartesian_cmd_type) {
     case ambf_msgs::RigidBodyCmd::TYPE_FORCE:{
-        afRBPtr->m_activeControllerType =  afControlType::FORCE;
+        afRBPtr->m_activeControllerType = afControlType::FORCE;
         if (afRBPtr->m_bulletRigidBody){
             force.setValue(afCommand.wrench.force.x,
                            afCommand.wrench.force.y,