Fixed bug in the reset logic for CDynamics3DModel and derived classes.

src/plugins/robots/prototype/simulator/dynamics3d_prototype_model.cpp

@@ -265,7 +265,7 @@ namespace argos {
 
          }
       }
-      /* With this call to reset, the multi-body model is configured and ready to be added */
+      /* Finalize model with a reset */
       Reset();
    }
 

src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_box_model.cpp

@@ -59,8 +59,8 @@ namespace argos {
       m_ptrBody = std::make_shared<CBody>(*this, &sAnchor, ptrShape, sData);
       /* Transfer the body to the base class */
       m_vecBodies.push_back(m_ptrBody);
-      /* Synchronize with the entity in the space */
-      UpdateEntityStatus();
+      /* Finalize model with a reset */
+      Reset();
    }
 
    /****************************************/

src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_cylinder_model.cpp

@@ -59,8 +59,8 @@ namespace argos {
       m_ptrBody = std::make_shared<CBody>(*this, &sAnchor, ptrShape, sData);
       /* Transfer the body to the base class */
       m_vecBodies.push_back(m_ptrBody);
-      /* Synchronize with the entity in the space */
-      UpdateEntityStatus();
+      /* Finalize model with a reset */
+      Reset();
    }
 
    /****************************************/

src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_engine.cpp

@@ -80,8 +80,9 @@ namespace argos {
              ++itPlugin) {
             itPlugin->second->UnregisterModel(*itModel->second);
          }
-         /* Reset the model */
+         /* Remove model from world */
          itModel->second->RemoveFromWorld(m_cWorld);
+         /* Reset the model */
          itModel->second->Reset();
       }
       /* Run the destructors on bullet's components */

src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_model.cpp

@@ -146,11 +146,11 @@ namespace argos {
    /****************************************/
 
    void CDynamics3DModel::UpdateEntityStatus() {
-      /* Update the anchor associated with each body before running the base class's UpdateEntityStatus
-         which updates the bounding box and origin anchor */
+      /* Update the anchor associated with each body */
       for(const std::shared_ptr<CAbstractBody>& ptr_body : m_vecBodies) {
          ptr_body->UpdateAnchor();
       }
+      /* Call CPhysicsModel::UpdateEntityStatus which updates the AABB and origin anchor */
       CPhysicsModel::UpdateEntityStatus();
    }
 

src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_multi_body_object_model.cpp

@@ -46,6 +46,12 @@ namespace argos {
       for(const std::shared_ptr<CAbstractBody>& ptr_body : m_vecBodies) {
          ptr_body->Reset();
       }
+      /* TODO. For the moment, we rely on the class that derives from
+         this model to call UpdateEntityStatus after the joints
+         have been configured. This is ugly and should be fixed
+         by moving the code for joints into this class */
+      /* Synchronize with the entity in the space */
+      //UpdateEntityStatus();
    }
 
    /****************************************/