From d0e049c0e1d7880cbae8305a0bc24f5c11e85d9d Mon Sep 17 00:00:00 2001
From: Henning Wiedemann <henning.wiedemann@dfki.de>
Date: Fri, 1 Mar 2019 11:20:05 +0100
Subject: [PATCH] Fix reset problem, where the joint ids change

---
 sim/src/core/EntityManager.cpp |  8 +++++---
 sim/src/core/JointManager.cpp  | 18 ++++++++++++++++++
 sim/src/core/SimEntity.cpp     |  7 ++++++-
 sim/src/core/SimEntity.h       |  2 ++
 4 files changed, 31 insertions(+), 4 deletions(-)

diff --git a/sim/src/core/EntityManager.cpp b/sim/src/core/EntityManager.cpp
index 166d465db..77c51a6ed 100644
--- 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
index b447f394b..3853d9773 100644
--- a/sim/src/core/JointManager.cpp
+++ b/sim/src/core/JointManager.cpp
@@ -115,6 +115,24 @@ namespace mars {
         // set the next free id
         jointS->index = next_joint_id;
         next_joint_id++;
+        fprintf(stderr, "%s\n", jointS->config.toYamlString().c_str());
+        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
index e0c16592a..2d40adeb4 100644
--- 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
index e6221f213..f118ebf36 100644
--- 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);