Fix reset problem, where joints have been loaded in wrong order

rock-simulation · Mar 1, 2019 · b3af26f · b3af26f
1 parent 9016c92
commit b3af26f
Show file tree

Hide file tree

Showing 4 changed files with 30 additions and 4 deletions.
diff --git a/sim/src/core/EntityManager.cpp b/sim/src/core/EntityManager.cpp
@@ -331,9 +331,11 @@ namespace mars {
     }
 
     void EntityManager::resetPose() {
-      std::map<unsigned long, SimEntity*>::iterator iter = entities.begin();
-      for (; iter != entities.end(); ++iter) {
-        iter->second->setInitialPose(true);
+      for (auto iter: entities) {
+        iter.second->removeAnchor();
+      }
+      for (auto iter: entities) {
+        iter.second->setInitialPose(true);
       }
     }
 

diff --git a/sim/src/core/JointManager.cpp b/sim/src/core/JointManager.cpp
@@ -115,6 +115,23 @@ namespace mars {
         // set the next free id
         jointS->index = next_joint_id;
         next_joint_id++;
+        if (jointS->config.hasKey("desired_id"))
+        {
+          unsigned long des_id=jointS->config["desired_id"];
+          bool found = false;
+          for (auto j : simJoints)
+          {
+            if (j.first == des_id) {
+              found = true;
+            }
+          }
+          if (!found) {
+            jointS->index = des_id;
+            next_joint_id--;
+            if (des_id>= next_joint_id)
+              next_joint_id = des_id + 1;
+          }
+        }
         SimJoint* newJoint = new SimJoint(control, *jointS);
         newJoint->setAttachedNodes(node1, node2);
         //    newJoint->setSJoint(*jointS);

diff --git a/sim/src/core/SimEntity.cpp b/sim/src/core/SimEntity.cpp
@@ -317,6 +317,10 @@ namespace mars {
       }
     }
 
+    void SimEntity::removeAnchor() {
+      if (hasAnchorJoint()) control->joints->removeJoint(anchorJointId);
+    }
+
     bool SimEntity::hasAnchorJoint() {
       return (anchorJointId != 0);
     }
@@ -438,7 +442,8 @@ namespace mars {
           anchorjoint.anchorPos = ANCHOR_NODE1;
           anchorjoint.type = JOINT_TYPE_FIXED;
           anchorjoint.name = "anchor_"+name;
-          anchorJointId = control->joints->addJoint(&anchorjoint, true);
+          if (hasAnchorJoint()) anchorjoint.config["desired_id"] = anchorJointId;
+          anchorJointId = control->joints->addJoint(&anchorjoint, hasAnchorJoint());
           addJoint(anchorJointId, anchorjoint.name);
         } else {
           //if there was a joint before, remove it

diff --git a/sim/src/core/SimEntity.h b/sim/src/core/SimEntity.h
@@ -148,6 +148,8 @@ namespace mars {
 
       const configmaps::ConfigMap getConfig();
 
+      void removeAnchor();
+
       bool hasAnchorJoint();
 
       void setInitialPose(bool reset=false/*compatibility*/, configmaps::ConfigMap* pPoseCfg=NULL);