Fix plane pose of OdeCollisionDetector

dartsim · Mar 25, 2017 · cb7572a · cb7572a
1 parent 0059d9e
commit cb7572a
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 5 deletions.
diff --git a/dart/collision/ode/OdeCollisionDetector.cpp b/dart/collision/ode/OdeCollisionDetector.cpp
@@ -296,7 +296,7 @@ dGeomID OdeCollisionDetector::createOdeCollisionGeometry(
 
     // TODO(JS): transform the normal and offset according to the transform
     // of the parent body.
-    odeGeomId = dCreatePlane(0, normal.x(), normal.y(), normal.z(), -0.375);
+    odeGeomId = dCreatePlane(0, normal.x(), normal.y(), normal.z(), offset);
   }
   //else if (MeshShape::getStaticType() == shapeType)
   //{

diff --git a/dart/collision/ode/OdeCollisionObject.cpp b/dart/collision/ode/OdeCollisionObject.cpp
@@ -46,13 +46,13 @@ OdeCollisionObject::GeomUserData::GeomUserData(
 //==============================================================================
 OdeCollisionObject::~OdeCollisionObject()
 {
-  dGeomDestroy(mGeomId);
-
   if (mBodyId)
   {
     dBodyDestroy(mBodyId);
     mBodyId = nullptr;
   }
+
+  dGeomDestroy(mGeomId);
 }
 
 //==============================================================================
@@ -83,9 +83,32 @@ OdeCollisionObject::OdeCollisionObject(
 //==============================================================================
 void OdeCollisionObject::updateEngineData()
 {
-  // If body id is nullptr, this object is immobile.
-  if (!mBodyId)
+  if (mShapeFrame->getShape()->is<dynamics::PlaneShape>())
+  {
+    // This code block is for immobile geom.
+    assert(!mBodyId);
+
+    const Eigen::Isometry3d& tf = getTransform();
+    const Eigen::Vector3d pos = tf.translation();
+    const Eigen::Matrix3d rot = tf.linear();
+
+    auto plane = static_cast<const dynamics::PlaneShape*>(
+          mShapeFrame->getShape().get());
+    const Eigen::Vector3d& normal = plane->getNormal();
+    const double offset = plane->getOffset();
+
+    const Eigen::Vector3d& normal2 = rot*normal;
+    const double offset2 = offset + pos.dot(normal2);
+
+    dGeomPlaneSetParams(
+        mGeomId, normal2.x(), normal2.y(), normal2.z(), offset2);
+
     return;
+  }
+
+  // If body id is nullptr, this object is immobile. All the immobile geom type
+  // must be handled above.
+  assert(mBodyId);
 
   const Eigen::Isometry3d& tf = getTransform();
 

diff --git a/dart/collision/ode/OdeCollisionObject.hpp b/dart/collision/ode/OdeCollisionObject.hpp
@@ -74,6 +74,8 @@ class OdeCollisionObject : public CollisionObject
   dGeomID mGeomId;
 
   /// ODE body id associated with this object
+  ///
+  /// If the ODE geom type is immobile, this is nullptr.
   dBodyID mBodyId;
 };