Merge pull request #308 from jhwangbo/master

now updating collisiong pos updates col vis. but not vis obj

raisim/linux/include/raisim/RaisimServer.hpp

@@ -954,31 +954,44 @@ class RaisimServer final {
         data_ = set(data_, (uint64_t) (dynamic_cast<ArticulatedSystem *>(ob)->getVisOb().size() +
                                       dynamic_cast<ArticulatedSystem *>(ob)->getVisColOb().size()));
 
-        for (uint64_t i = 0; i < 2; i++) {
-          std::vector<VisObject> *visOb;
-          if (i == 0)
-            visOb = &(dynamic_cast<ArticulatedSystem *>(ob)->getVisOb());
-          else
-            visOb = &(dynamic_cast<ArticulatedSystem *>(ob)->getVisColOb());
-
-          for (uint64_t k = 0; k < (*visOb).size(); k++) {
-            auto &vob = (*visOb)[k];
-            std::string name = std::to_string(ob->getIndexInWorld()) +
-                "/" + std::to_string(i) + "/" +
-                std::to_string(k);
-            data_ = set(data_, name);
-
-            Vec<3> pos, offsetInWorld;
-            Vec<4> quat;
-            Mat<3, 3> bodyRotation, rot;
-            ob->getPosition(vob.localIdx, pos);
-            ob->getOrientation(vob.localIdx, bodyRotation);
-            matvecmul(bodyRotation, vob.offset, offsetInWorld);
-            matmul(bodyRotation, vob.rot, rot);
-            raisim::rotMatToQuat(rot, quat);
-            pos = pos + offsetInWorld;
-            data_ = set(data_, pos, quat);
-          }
+        auto& visOb = dynamic_cast<ArticulatedSystem *>(ob)->getVisOb();
+        for (uint64_t k = 0; k < visOb.size(); k++) {
+          auto &vob = visOb[k];
+          std::string name = std::to_string(ob->getIndexInWorld()) +
+                             "/" + std::to_string(0) + "/" +
+                             std::to_string(k);
+          data_ = set(data_, name);
+
+          Vec<3> pos, offsetInWorld;
+          Vec<4> quat;
+          Mat<3, 3> bodyRotation, rot;
+          ob->getPosition(vob.localIdx, pos);
+          ob->getOrientation(vob.localIdx, bodyRotation);
+          matvecmul(bodyRotation, vob.offset, offsetInWorld);
+          matmul(bodyRotation, vob.rot, rot);
+          raisim::rotMatToQuat(rot, quat);
+          pos = pos + offsetInWorld;
+          data_ = set(data_, pos, quat);
+        }
+
+        auto& colOb = dynamic_cast<ArticulatedSystem *>(ob)->getCollisionBodies();
+        for (uint64_t k = 0; k < colOb.size(); k++) {
+          auto &vob = colOb[k];
+          std::string name = std::to_string(ob->getIndexInWorld()) +
+                             "/" + std::to_string(1) + "/" +
+                             std::to_string(k);
+          data_ = set(data_, name);
+
+          Vec<3> pos, offsetInWorld;
+          Vec<4> quat;
+          Mat<3, 3> bodyRotation, rot;
+          ob->getPosition(vob.localIdx, pos);
+          ob->getOrientation(vob.localIdx, bodyRotation);
+          matvecmul(bodyRotation, vob.posOffset, offsetInWorld);
+          matmul(bodyRotation, vob.rotOffset, rot);
+          raisim::rotMatToQuat(rot, quat);
+          pos = pos + offsetInWorld;
+          data_ = set(data_, pos, quat);
         }
       } else if (ob->getObjectType() == ObjectType::COMPOUND) {
         auto *com = dynamic_cast<Compound *>(ob);

raisim/linux/include/raisim/object/ArticulatedSystem/ArticulatedSystem.hpp

@@ -905,7 +905,7 @@ class ArticulatedSystem : public Object {
   const std::vector<raisim::Vec<3>> &getBodyCOM_B() const { return comPos_B; }
 
   /**
- * @return a reference to the position of the center of the mass of each body in the world frame.*/
+   * @return a reference to the position of the center of the mass of each body in the world frame.*/
   std::vector<raisim::Vec<3>> &getBodyCOM_W() { return comPos_W; }
   const std::vector<raisim::Vec<3>> &getBodyCOM_W() const { return comPos_W; }