Testing ContactConstraint for frictionless contacts

Migration.md

@@ -93,6 +93,12 @@
   + virtual void integrateConfigs(const Eigen::VectorXd& _genVels, double _dt) = 0
   + virtual void integrateGenVels(const Eigen::VectorXd& _genAccs, double _dt) = 0
 
+1. **dart/utils/SkelParser.h**
+  + static dynamics::Skeleton* readSkeleton(const std::string& _filename)
+
+1. **dart/utils/SoftSkelParser.h**
+  + static dynamics::SoftSkeleton* readSoftSkeleton(const std::string& _filename)
+
 ### Deletions
 
 1. **dart/dynamics/BodyNode.h**

apps/CMakeLists.txt

@@ -7,6 +7,7 @@ foreach(APPDIR
     balance
     ballJointConstraintTest
     closedLoop
+    cube
     cubes
     doublePendulumWithBase
     forwardSim

apps/cube/CMakeLists.txt

@@ -0,0 +1,7 @@
+###############################################
+# apps/cube
+file(GLOB cube_srcs "*.cpp")
+file(GLOB cube_hdrs "*.h")
+add_executable(cube ${cube_srcs} ${cube_hdrs})
+target_link_libraries(cube dart)
+set_target_properties(cube PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin")

apps/cube/Main.cpp

@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2011-2013, Georgia Tech Research Corporation
+ * All rights reserved.
+ *
+ * Author(s): Karen Liu <[email protected]>,
+ *            Jeongseok Lee <[email protected]>
+ *
+ * Georgia Tech Graphics Lab and Humanoid Robotics Lab
+ *
+ * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
+ * <[email protected]> <[email protected]>
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <iostream>
+
+#include "dart/simulation/World.h"
+#include "dart/utils/Paths.h"
+#include "dart/utils/SkelParser.h"
+#include "dart/constraint/ConstraintSolver.h"
+#include "apps/cubes/MyWindow.h"
+
+int main(int argc, char* argv[]) {
+  // create and initialize the world
+  dart::simulation::World *myWorld
+      = dart::utils::SkelParser::readSkelFile(DART_DATA_PATH"/skel/cube.skel");
+  assert(myWorld != NULL);
+  Eigen::Vector3d gravity(0.0, -9.81, 0.0);
+  myWorld->setGravity(gravity);
+
+  // create a window and link it to the world
+  MyWindow window;
+  window.setWorld(myWorld);
+
+  std::cout << "space bar: simulation on/off" << std::endl;
+  std::cout << "'p': playback/stop" << std::endl;
+  std::cout << "'[' and ']': play one frame backward and forward" << std::endl;
+  std::cout << "'v': visualization on/off" << std::endl;
+  std::cout << "'1'--'4': programmed interaction" << std::endl;
+  std::cout << "'q': spawn a random cube" << std::endl;
+  std::cout << "'w': delete a spawned cube" << std::endl;
+
+  glutInit(&argc, argv);
+  window.initWindow(640, 480, "Boxes");
+  glutMainLoop();
+
+  return 0;
+}

apps/cube/MyWindow.cpp

@@ -0,0 +1,161 @@
+/*
+ * Copyright (c) 2011-2013, Georgia Tech Research Corporation
+ * All rights reserved.
+ *
+ * Author(s): Karen Liu <[email protected]>,
+ *            Jeongseok Lee <[email protected]>
+ *
+ * Georgia Tech Graphics Lab and Humanoid Robotics Lab
+ *
+ * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
+ * <[email protected]> <[email protected]>
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "apps/cubes/MyWindow.h"
+
+#include "dart/math/Helpers.h"
+#include "dart/simulation/World.h"
+#include "dart/dynamics/BodyNode.h"
+#include "dart/dynamics/Skeleton.h"
+#include "dart/dynamics/FreeJoint.h"
+#include "dart/dynamics/BoxShape.h"
+
+MyWindow::MyWindow()
+  : SimWindow() {
+  mForce = Eigen::Vector3d::Zero();
+}
+
+MyWindow::~MyWindow() {
+}
+
+void MyWindow::timeStepping() {
+  mWorld->getSkeleton(1)->getBodyNode(0)->addExtForce(mForce);
+  mWorld->step();
+  mForce /= 2.0;
+}
+
+void MyWindow::drawSkels() {
+  glEnable(GL_LIGHTING);
+  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+
+  SimWindow::drawSkels();
+}
+
+void MyWindow::keyboard(unsigned char _key, int _x, int _y) {
+  switch (_key) {
+    case ' ':  // use space key to play or stop the motion
+      mSimulating = !mSimulating;
+      if (mSimulating) {
+        mPlay = false;
+        glutTimerFunc(mDisplayTimeout, refreshTimer, 0);
+      }
+      break;
+    case 'p':  // playBack
+      mPlay = !mPlay;
+      if (mPlay) {
+        mSimulating = false;
+        glutTimerFunc(mDisplayTimeout, refreshTimer, 0);
+      }
+      break;
+    case '[':  // step backward
+      if (!mSimulating) {
+        mPlayFrame--;
+        if (mPlayFrame < 0)
+          mPlayFrame = 0;
+        glutPostRedisplay();
+      }
+      break;
+    case ']':  // step forwardward
+      if (!mSimulating) {
+        mPlayFrame++;
+        if (mPlayFrame >= mBakedStates.size())
+          mPlayFrame = 0;
+        glutPostRedisplay();
+      }
+      break;
+    case 'v':  // show or hide markers
+      mShowMarkers = !mShowMarkers;
+      break;
+    case '1':  // upper right force
+      mForce[0] = -500;
+      break;
+    case '2':  // upper right force
+      mForce[0] = 500;
+      break;
+    case '3':  // upper right force
+      mForce[2] = -500;
+      break;
+    case '4':  // upper right force
+      mForce[2] = 500;
+      break;
+    case 'q':  // Spawn a cube
+    case 'Q': {  // Spawn a cube
+      Eigen::Vector3d position = Eigen::Vector3d(dart::math::random(-1.0, 1.0),
+                                                 dart::math::random(-1.0, 1.0),
+                                                 dart::math::random(0.5, 1.0));
+      Eigen::Vector3d size = Eigen::Vector3d(dart::math::random(0.01, 0.2),
+                                             dart::math::random(0.01, 0.2),
+                                             dart::math::random(0.01, 0.2));
+      spawnCube(position, size);
+      break;
+    }
+    case 'w':    // Spawn a cube
+    case 'W': {  // Spawn a cube
+      if (mWorld->getNumSkeletons() > 4)
+        mWorld->removeSkeleton(mWorld->getSkeleton(4));
+      break;
+    }
+    default:
+      Win3D::keyboard(_key, _x, _y);
+  }
+  glutPostRedisplay();
+}
+
+void MyWindow::spawnCube(const Eigen::Vector3d& _position,
+                         const Eigen::Vector3d& _size,
+                         double _mass) {
+  dart::dynamics::Skeleton*  newCubeSkeleton =
+      new dart::dynamics::Skeleton();
+  dart::dynamics::BodyNode*  newBodyNode     =
+      new dart::dynamics::BodyNode("cube_link");
+  dart::dynamics::FreeJoint* newFreeJoint    =
+      new dart::dynamics::FreeJoint("cube_joint");
+  dart::dynamics::BoxShape*  newBoxShape     =
+      new dart::dynamics::BoxShape(_size);
+
+  newBodyNode->addVisualizationShape(newBoxShape);
+  newBodyNode->addCollisionShape(newBoxShape);
+  newBodyNode->setMass(_mass);
+  newBodyNode->setParentJoint(newFreeJoint);
+  newFreeJoint->setTransformFromParentBodyNode(
+        Eigen::Isometry3d(Eigen::Translation3d(_position)));
+  newBoxShape->setColor(Eigen::Vector3d(dart::math::random(0.0, 1.0),
+                                        dart::math::random(0.0, 1.0),
+                                        dart::math::random(0.0, 1.0)));
+  newCubeSkeleton->addBodyNode(newBodyNode);
+  mWorld->addSkeleton(newCubeSkeleton);
+}

apps/cube/MyWindow.h

@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2011-2013, Georgia Tech Research Corporation
+ * All rights reserved.
+ *
+ * Author(s): Karen Liu <[email protected]>,
+ *            Jeongseok Lee <[email protected]>
+ *
+ * Georgia Tech Graphics Lab and Humanoid Robotics Lab
+ *
+ * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
+ * <[email protected]> <[email protected]>
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef APPS_CUBES_MYWINDOW_H_
+#define APPS_CUBES_MYWINDOW_H_
+
+#include "dart/gui/SimWindow.h"
+
+/// \brief
+class MyWindow : public dart::gui::SimWindow {
+public:
+  /// \brief
+  MyWindow();
+
+  /// \brief
+  virtual ~MyWindow();
+
+  /// \brief
+  virtual void timeStepping();
+
+  /// \brief
+  virtual void drawSkels();
+
+  /// \brief
+  virtual void keyboard(unsigned char _key, int _x, int _y);
+
+  /// \brief
+  void spawnCube(
+      const Eigen::Vector3d& _position = Eigen::Vector3d(0.0, 1.0, 0.0),
+      const Eigen::Vector3d& _size     = Eigen::Vector3d(0.1, 0.1, 0.1),
+      double _mass = 1.0);
+
+private:
+  /// \brief
+  Eigen::Vector3d mForce;
+
+  /// \brief Number of frames for applying external force
+  int mImpulseDuration;
+};
+
+#endif  // APPS_CUBES_MYWINDOW_H_

dart/collision/CollisionDetector.cpp

@@ -100,9 +100,10 @@ void CollisionDetector::removeSkeleton(dynamics::Skeleton* _skeleton)
 //==============================================================================
 void CollisionDetector::removeAllSkeletons()
 {
-  std::cout << "CollisionDetector::removeAllSkeletons(): "
-            << "Not implemented yet."
-            << std::endl;
+  for (size_t i = 0; i < mSkeletons.size(); ++i)
+    removeSkeleton(mSkeletons[i]);
+
+  mSkeletons.clear();
 }
 
 void CollisionDetector::addCollisionSkeletonNode(dynamics::BodyNode* _bodyNode,