Add pyramid shape support (#1466)

.azure-pipelines.yml

@@ -26,18 +26,6 @@ jobs:
   steps:
   - template: .ci/azure-pipelines/docker.yml
 
-- job: eoan_gcc_debug
-  timeoutInMinutes: 120
-  pool:
-    vmImage: 'ubuntu-16.04'
-  variables:
-    COMPILER: gcc
-    BUILD_TYPE: Debug
-    BUILD_DIR: $(Build.SourcesDirectory)
-    DOCKERFILE: Dockerfile.ubuntu-eoan
-  steps:
-  - template: .ci/azure-pipelines/docker.yml
-
 - job: focal_gcc_debug
   timeoutInMinutes: 120
   pool:
@@ -50,7 +38,7 @@ jobs:
   steps:
   - template: .ci/azure-pipelines/docker.yml
 
-- job: mojav_clang_debug
+- job: catalina_clang_debug
   timeoutInMinutes: 120
   pool:
     vmImage: 'macOS-10.15'

.ci/install_linux.sh

@@ -66,7 +66,7 @@ if [ $(lsb_release -sc) = "xenial" ] || [ $(lsb_release -sc) = "bionic" ]; then
     liboctomap-dev \
     libode-dev \
     clang-format-6.0
-elif [ $(lsb_release -sc) = "eoan" ] || [ $(lsb_release -sc) = "focal" ]; then
+elif [ $(lsb_release -sc) = "focal" ]; then
   apt-get install -y --no-install-recommends \
     libnlopt-cxx-dev \
     liboctomap-dev \
@@ -81,7 +81,8 @@ if [ "$BUILD_DARTPY" = "ON" ]; then
   apt-get install -y --no-install-recommends \
     python3-dev \
     python3-numpy \
-    python3-pip
+    python3-pip \
+    python3-setuptools
   pip3 install pytest -U
 
   if [ $(lsb_release -sc) = "xenial" ] || [ $(lsb_release -sc) = "bionic" ]; then
@@ -93,7 +94,7 @@ if [ "$BUILD_DARTPY" = "ON" ]; then
     make -j4
     make install
     cd ../..
-  elif [ $(lsb_release -sc) = "eoan" ] || [ $(lsb_release -sc) = "focal" ]; then
+  elif [ $(lsb_release -sc) = "focal" ]; then
     apt-get -y install pybind11-dev python3 libpython3-dev python3-pytest \
       python3-distutils
   else

.github/ISSUE_TEMPLATE/bug_report.md

@@ -17,7 +17,7 @@ assignees: ''
 #### Environment
 > Select the following information.
 * DART version: [e.g., master, 6.8.3]
-* OS name and version name(or number): [e.g., Ubuntu 18.04, macOS Mojav, Windows 10]
+* OS name and version name(or number): [e.g., Ubuntu 18.04, macOS Catalina, Windows 10]
 * Compiler name and version number: [e.g., GCC 7.4.0, Clang 3.9.1]
 
 #### Expected Behavior

.github/workflows/ccpp.yml

@@ -69,43 +69,6 @@ jobs:
       run: |
         docker exec dart-docker /bin/sh -c "cd $GITHUB_WORKSPACE && ./.ci/script.sh";
 
-  eoan_gcc_release:
-    name: Ubuntu 19.10 [GCC|Release]
-    runs-on: ubuntu-18.04
-    steps:
-    - uses: actions/checkout@v1
-    - name: Install Dependencies
-      env:
-        COMPILER: gcc
-        DOCKERFILE: Dockerfile.ubuntu-eoan
-        BUILD_TYPE: Release
-      run: |
-        docker build -t "${DOCKERFILE,,}" -f ".ci/docker/$DOCKERFILE" .;
-        docker run -itd -v $GITHUB_WORKSPACE:$GITHUB_WORKSPACE --env-file ./.ci/docker/env.list --name dart-docker "${DOCKERFILE,,}";
-        docker exec dart-docker /bin/sh -c "cd $GITHUB_WORKSPACE && ./.ci/install.sh";
-    - name: Build
-      run: |
-        docker exec dart-docker /bin/sh -c "cd $GITHUB_WORKSPACE && ./.ci/script.sh";
-
-  eoan_gcc_dartpy_release:
-    name: Ubuntu 19.10 [GCC|dartpy|Release]
-    runs-on: ubuntu-18.04
-    steps:
-    - uses: actions/checkout@v1
-    - name: Install Dependencies
-      env:
-        COMPILER: gcc
-        DOCKERFILE: Dockerfile.ubuntu-eoan
-        BUILD_DARTPY: ON
-        BUILD_TYPE: Release
-      run: |
-        docker build -t "${DOCKERFILE,,}" -f ".ci/docker/$DOCKERFILE" .;
-        docker run -itd -v $GITHUB_WORKSPACE:$GITHUB_WORKSPACE --env-file ./.ci/docker/env.list --name dart-docker "${DOCKERFILE,,}";
-        docker exec dart-docker /bin/sh -c "cd $GITHUB_WORKSPACE && ./.ci/install.sh";
-    - name: Build
-      run: |
-        docker exec dart-docker /bin/sh -c "cd $GITHUB_WORKSPACE && ./.ci/script.sh";
-
   focal_gcc_release:
     name: Ubuntu 20.04 [GCC|Release]
     runs-on: ubuntu-18.04
@@ -143,7 +106,7 @@ jobs:
       run: |
         docker exec dart-docker /bin/sh -c "cd $GITHUB_WORKSPACE && ./.ci/script.sh";
 
-  mojav_clang_release:
+  catalina_clang_release:
     name: macOS 10.15 [Clang|Release]
     runs-on: macOS-10.15
     steps:
@@ -158,7 +121,7 @@ jobs:
         BUILD_TYPE: Release
       run: sudo -E .ci/script.sh
 
-  mojav_clang_dartpy_release:
+  catalina_clang_dartpy_release:
     name: macOS 10.15 [Clang|dartpy|Release]
     runs-on: macOS-10.15
     steps:

CHANGELOG.md

@@ -11,6 +11,7 @@
 
   * Added ConeShape support for FCLCollisionDetector: [#1447](https://github.com/dartsim/dart/pull/1447)
   * Fixed segfault from raycast when no ray hit: [#1461](https://github.com/dartsim/dart/pull/1461)
+  * Added PyramidShape class: [#1466](https://github.com/dartsim/dart/pull/1466)
 
 * Kinematics
 

dart/collision/fcl/FCLCollisionDetector.cpp

@@ -48,6 +48,7 @@
 #include "dart/dynamics/EllipsoidShape.hpp"
 #include "dart/dynamics/MeshShape.hpp"
 #include "dart/dynamics/PlaneShape.hpp"
+#include "dart/dynamics/PyramidShape.hpp"
 #include "dart/dynamics/Shape.hpp"
 #include "dart/dynamics/ShapeFrame.hpp"
 #include "dart/dynamics/SoftMeshShape.hpp"
@@ -487,6 +488,65 @@ ::fcl::BVHModel<BV>* createCylinder(
   return model;
 }
 
+//==============================================================================
+template <typename BV>
+::fcl::BVHModel<BV>* createPyramid(
+    const dynamics::PyramidShape& shape, const fcl::Transform3& pose)
+{
+  ::fcl::BVHModel<BV>* model = new ::fcl::BVHModel<BV>;
+
+  std::vector<fcl::Vector3> points(5);
+  std::vector<::fcl::Triangle> faces(6);
+
+  const double w = shape.getBaseWidth();
+  const double d = shape.getBaseDepth();
+  const double h = shape.getHeight();
+
+  const double hTop = h / 2;
+  const double hBottom = -hTop;
+  const double left = -w / 2;
+  const double right = w / 2;
+  const double front = -d / 2;
+  const double back = d / 2;
+
+#if FCL_VERSION_AT_LEAST(0, 6, 0)
+  points[0] << 0, 0, hTop;
+  points[1] << right, back, hBottom;
+  points[2] << left, back, hBottom;
+  points[3] << left, front, hBottom;
+  points[4] << right, front, hBottom;
+#else
+  points[0].setValue(0, 0, hTop);
+  points[1].setValue(right, back, hBottom);
+  points[2].setValue(left, back, hBottom);
+  points[3].setValue(left, front, hBottom);
+  points[4].setValue(right, front, hBottom);
+#endif
+
+  faces[0].set(0, 1, 2);
+  faces[1].set(0, 2, 3);
+  faces[2].set(0, 3, 4);
+  faces[3].set(0, 4, 1);
+  faces[4].set(1, 3, 2);
+  faces[5].set(1, 4, 3);
+
+  for (unsigned int i = 0; i < points.size(); ++i)
+  {
+#if FCL_VERSION_AT_LEAST(0, 6, 0)
+    points[i] = pose * points[i];
+#else
+    points[i] = pose.transform(points[i]);
+#endif
+  }
+
+  model->beginModel();
+  model->addSubModel(points, faces);
+  model->endModel();
+  model->computeLocalAABB();
+
+  return model;
+}
+
 //==============================================================================
 template <class BV>
 ::fcl::BVHModel<BV>* createMesh(
@@ -837,6 +897,7 @@ FCLCollisionDetector::createFCLCollisionGeometry(
   using dynamics::EllipsoidShape;
   using dynamics::MeshShape;
   using dynamics::PlaneShape;
+  using dynamics::PyramidShape;
   using dynamics::Shape;
   using dynamics::SoftMeshShape;
   using dynamics::SphereShape;
@@ -942,6 +1003,14 @@ FCLCollisionDetector::createFCLCollisionGeometry(
       geom = fclMesh;
     }
   }
+  else if (PyramidShape::getStaticType() == shapeType)
+  {
+    assert(dynamic_cast<const PyramidShape*>(shape.get()));
+
+    const auto pyramid = std::static_pointer_cast<const PyramidShape>(shape);
+    // Use mesh since FCL doesn't support pyramid shape.
+    geom = createPyramid<fcl::OBBRSS>(*pyramid, fcl::Transform3());
+  }
   else if (PlaneShape::getStaticType() == shapeType)
   {
     if (FCLCollisionDetector::PRIMITIVE == type)

dart/constraint/BoxedLcpConstraintSolver.cpp

@@ -102,7 +102,7 @@ void BoxedLcpConstraintSolver::setBoxedLcpSolver(BoxedLcpSolverPtr lcpSolver)
   if (!lcpSolver)
   {
     dtwarn << "[BoxedLcpConstraintSolver::setBoxedLcpSolver] "
-           << "nullptr for boxed LCP solver is not allowed.";
+           << "nullptr for boxed LCP solver is not allowed.\n";
     return;
   }