Fix resetting of joints including anchor joints

rock-simulation · Aug 27, 2015 · 46ed0d4 · 46ed0d4
1 parent 9526b94
commit 46ed0d4
Show file tree

Hide file tree

Showing 5 changed files with 35 additions and 13 deletions.
diff --git a/entity_generation/smurf/src/smurf.cpp b/entity_generation/smurf/src/smurf.cpp
@@ -949,18 +949,15 @@ namespace mars {
         if (!loadGraphic(graphicList[i]))
           return 0;
 
-      setPose();
-
-      control->nodes->printNodeMasses(true);
-
-      return 1;
-    }
-
-    void SMURF::setPose() {
+      // set model pose
       ConfigMap map;
       map["rootNode"] = model->root_link_->name;
       entity->appendConfig(map);
       entity->setInitialPose();
+
+      control->nodes->printNodeMasses(true);
+
+      return 1;
     }
 
     unsigned int SMURF::loadNode(ConfigMap config) {

diff --git a/entity_generation/smurf/src/smurf.h b/entity_generation/smurf/src/smurf.h
@@ -139,7 +139,6 @@ namespace mars {
       void poseToVectorAndQuaternion(const urdf::Pose &pose, utils::Vector *v, utils::Quaternion *q);
       bool isEqualPos(const urdf::Pose &p1, const urdf::Pose p2);
       bool isNullPos(const urdf::Pose &p);
-      void setPose();     
 
       // load functions
       unsigned int loadMaterial(configmaps::ConfigMap config);

diff --git a/sim/src/core/EntityManager.cpp b/sim/src/core/EntityManager.cpp
@@ -231,7 +231,7 @@ namespace mars {
     void EntityManager::resetPose() {
       std::map<unsigned long, SimEntity*>::iterator iter = entities.begin();
       for (; iter != entities.end(); ++iter) {
-        iter->second->setInitialPose();
+        iter->second->setInitialPose(true);
       }
     }
 

diff --git a/sim/src/core/SimEntity.cpp b/sim/src/core/SimEntity.cpp
@@ -19,13 +19,15 @@
  */
 
 #include "SimEntity.h"
+#include "SimJoint.h"
 #include <configmaps/ConfigData.h>
 #include <iostream>
 #include <mars/utils/mathUtils.h>
 #include <mars/interfaces/NodeData.h>
 #include <mars/interfaces/sim/ControlCenter.h>
 #include <mars/interfaces/sim/SimulatorInterface.h>
 #include <mars/interfaces/sim/NodeManagerInterface.h>
+#include <mars/interfaces/sim/JointManagerInterface.h>
 #include <mars/interfaces/sim/MotorManagerInterface.h>
 #include <iterator> // ostream_iterator
 
@@ -172,7 +174,7 @@ namespace mars {
       }
     }
 
-    void SimEntity::setInitialPose() {
+    void SimEntity::setInitialPose(bool reset) {
       bool worldAnchor = false;
       if(!control) return;
       if(config.find("rootNode") != config.end()) {
@@ -215,7 +217,31 @@ namespace mars {
           control->nodes->editNode(&myNode, EDIT_NODE_ROT | EDIT_NODE_MOVE_ALL);
         }
         if(worldAnchor) {
-          control->sim->connectNodes(id, 0);
+          if (reset) {
+            fprintf(stderr, "Resetting initial entity pose.\n");
+            std::map<unsigned long, std::string>::iterator it;
+            JointId anchorJointId = 0;
+            for (it=jointIds.begin(); it!=jointIds.end(); ++it) {
+              if (it->second == "anchor_"+name) {
+                anchorJointId = it->first;
+                break;
+              }
+            }
+            SimJoint* anchorjoint = control->joints->getSimJoint(anchorJointId);
+            if (anchorjoint != NULL) {
+              anchorjoint->reattachJoint();
+            }
+            else fprintf(stderr, "Could not reset anchor of entity %s.\n", name.c_str());
+          }
+          else {
+            JointData anchorjoint;
+            anchorjoint.nodeIndex1 = id;
+            anchorjoint.nodeIndex2 = 0;
+            anchorjoint.type = JOINT_TYPE_FIXED;
+            anchorjoint.name = "anchor_"+name;
+            JointId anchorJointId = control->joints->addJoint(&anchorjoint);
+            addJoint(anchorJointId, anchorjoint.name);
+          }
         }
         // set Joints
         configmaps::ConfigVector::iterator it;

diff --git a/sim/src/core/SimEntity.h b/sim/src/core/SimEntity.h
@@ -115,7 +115,7 @@ namespace mars {
 
       std::string getJoint(unsigned long id);
 
-      void setInitialPose();
+      void setInitialPose(bool reset=false);
 
       //debug functions
       void printNodes();