Merge pull request #298 from dartsim/hybrid_dynamics

Hybrid dynamics

.travis.yml

@@ -1,6 +1,6 @@
 language: cpp
 compiler:
-  - gcc
+  #- gcc  # Disabled gcc version is 4.6.3 while DART requires 4.7.0 or greater
   - clang
 before_install:
   - 'ci/before_install.sh'

apps/CMakeLists.txt

@@ -7,6 +7,7 @@ foreach(APPDIR
     atlasSimbicon
     bipedStand
     hardcodedDesign    
+    hybridDynamics
     jointConstraints
     mixedChain
     operationalSpaceControl

apps/hybridDynamics/CMakeLists.txt

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

apps/hybridDynamics/Main.cpp

@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2014, Georgia Tech Research Corporation
+ * All rights reserved.
+ *
+ * Author(s): Jeongseok Lee <jslee02@gmail.com>
+ *
+ * Georgia Tech Graphics Lab and Humanoid Robotics Lab
+ *
+ * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
+ * <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
+ *
+ * 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/dart.h"
+#include "MyWindow.h"
+
+int main(int argc, char* argv[])
+{
+  // create and initialize the world
+  dart::simulation::World *myWorld
+      = dart::utils::SkelParser::readWorld(
+          DART_DATA_PATH"/skel/fullbody1.skel");
+  assert(myWorld != NULL);
+  Eigen::Vector3d gravity(0.0, -9.81, 0.0);
+  myWorld->setGravity(gravity);
+
+  dart::dynamics::Skeleton* skel  = myWorld->getSkeleton(1);
+
+  std::vector<size_t> genCoordIds;
+  genCoordIds.push_back(1);
+  genCoordIds.push_back(6);   // left hip
+  genCoordIds.push_back(14);  // left knee
+  genCoordIds.push_back(17);  // left ankle
+  genCoordIds.push_back(9);   // right hip
+  genCoordIds.push_back(15);  // right knee
+  genCoordIds.push_back(19);  // right ankle
+  genCoordIds.push_back(13);  // lower back
+  Eigen::VectorXd initConfig(8);
+  initConfig << -0.2, 0.15, -0.4, 0.25, 0.15, -0.4, 0.25, 0.0;
+  skel->setPositionSegment(genCoordIds, initConfig);
+  skel->computeForwardKinematics(true, true, false);
+
+  dart::dynamics::Joint* joint0 = skel->getJoint(0);
+  joint0->setActuatorType(dart::dynamics::Joint::PASSIVE);
+  for (size_t i = 1; i < skel->getNumBodyNodes(); ++i)
+  {
+    dart::dynamics::Joint* joint = skel->getJoint(i);
+    joint->setActuatorType(dart::dynamics::Joint::VELOCITY);
+  }
+
+  // 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 << "'h': harness on/off" << std::endl;
+
+  glutInit(&argc, argv);
+  window.initWindow(640, 480, "Hybrid Dynamics");
+  glutMainLoop();
+
+  return 0;
+}

apps/hybridDynamics/MyWindow.cpp

@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) 2014, Georgia Tech Research Corporation
+ * All rights reserved.
+ *
+ * Author(s): Jeongseok Lee <jslee02@gmail.com>
+ *
+ * Georgia Tech Graphics Lab and Humanoid Robotics Lab
+ *
+ * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
+ * <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
+ *
+ * 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 "MyWindow.h"
+
+//==============================================================================
+MyWindow::MyWindow()
+  : SimWindow(),
+    mHarnessOn(false)
+{
+}
+
+//==============================================================================
+MyWindow::~MyWindow()
+{
+}
+
+//==============================================================================
+void MyWindow::timeStepping()
+{
+  dart::dynamics::Skeleton* skel  = mWorld->getSkeleton(1);
+
+  size_t index0 = skel->getJoint("j_scapula_left")->getIndexInSkeleton(0);
+  size_t index1 = skel->getJoint("j_scapula_right")->getIndexInSkeleton(0);
+  size_t index2 = skel->getJoint("j_forearm_left")->getIndexInSkeleton(0);
+  size_t index3 = skel->getJoint("j_forearm_right")->getIndexInSkeleton(0);
+
+  size_t index6 = skel->getJoint("j_shin_left")->getIndexInSkeleton(0);
+  size_t index7 = skel->getJoint("j_shin_right")->getIndexInSkeleton(0);
+
+  skel->setCommand(index0,  1.0 * std::sin(mWorld->getTime() * 4.0));
+  skel->setCommand(index1, -1.0 * std::sin(mWorld->getTime() * 4.0));
+  skel->setCommand(index2,  0.8 * std::sin(mWorld->getTime() * 4.0));
+  skel->setCommand(index3,  0.8 * std::sin(mWorld->getTime() * 4.0));
+
+  skel->setCommand(index6,  0.1 * std::sin(mWorld->getTime() * 2.0));
+  skel->setCommand(index7,  0.1 * std::sin(mWorld->getTime() * 2.0));
+
+  mWorld->step();
+}
+
+//==============================================================================
+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 >= mWorld->getRecording()->getNumFrames())
+          mPlayFrame = 0;
+        glutPostRedisplay();
+      }
+      break;
+    case 'v':  // show or hide markers
+      mShowMarkers = !mShowMarkers;
+      break;
+    case 'h':
+      mHarnessOn = !mHarnessOn;
+      if (mHarnessOn)
+      {
+        dart::dynamics::Joint* joint
+            = mWorld->getSkeleton(1)->getBodyNode("h_pelvis")->getParentJoint();
+        joint->setActuatorType(dart::dynamics::Joint::LOCKED);
+        std::cout << "The pelvis is locked." << std::endl;
+      }
+      else
+      {
+        dart::dynamics::Joint* joint
+            = mWorld->getSkeleton(1)->getBodyNode("h_pelvis")->getParentJoint();
+        joint->setActuatorType(dart::dynamics::Joint::PASSIVE);
+        std::cout << "The pelvis is unlocked." << std::endl;
+      }
+      break;
+    default:
+      Win3D::keyboard(_key, _x, _y);
+  }
+  glutPostRedisplay();
+}

apps/hybridDynamics/MyWindow.h

@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2014, Georgia Tech Research Corporation
+ * All rights reserved.
+ *
+ * Author(s): Jeongseok Lee <jslee02@gmail.com>
+ *
+ * Georgia Tech Graphics Lab and Humanoid Robotics Lab
+ *
+ * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
+ * <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
+ *
+ * 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_HYBRIDDYNAMICS_MYWINDOW_H_
+#define APPS_HYBRIDDYNAMICS_MYWINDOW_H_
+
+#include "dart/dart.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);
+
+private:
+  bool mHarnessOn;
+};
+
+#endif  // APPS_HYBRIDDYNAMICS_MYWINDOW_H_

apps/jointConstraints/MyWindow.cpp

@@ -138,29 +138,22 @@ void MyWindow::keyboard(unsigned char key, int x, int y)
       break;
 
     case 'h':
+      mHarnessOn = !mHarnessOn;
       if (mHarnessOn)
       {
-        mWorld->getConstraintSolver()->removeConstraint(mWeldJoint);
-        mHarnessOn = false;
+        BodyNode* bd = mWorld->getSkeleton(1)->getBodyNode("h_pelvis");
+        mWeldJoint = new WeldJointConstraint(bd);
+        mWorld->getConstraintSolver()->addConstraint(mWeldJoint);
       }
       else
       {
-        addWeldConstraint();
+        mWorld->getConstraintSolver()->removeConstraint(mWeldJoint);
       }
       break;
-
     default:
       Win3D::keyboard(key,x,y);
 
   }
   glutPostRedisplay();
 }
 
-void MyWindow::addWeldConstraint()
-{
-  BodyNode* bd = mWorld->getSkeleton(1)->getBodyNode("h_pelvis");
-  mWeldJoint = new WeldJointConstraint(bd);
-  mWorld->getConstraintSolver()->addConstraint(mWeldJoint);
-
-  mHarnessOn = true;
-}

apps/jointConstraints/MyWindow.h

@@ -73,7 +73,6 @@ class MyWindow : public dart::gui::SimWindow
   Controller* mController;
   dart::constraint::WeldJointConstraint* mWeldJoint;
   int mImpulseDuration;
-  void addWeldConstraint();
   bool mHarnessOn;
 
 };