Merge pull request #319 from dartsim/grey/body_frames

Frame class and auto-updating for forward kinematics

.travis.yml

@@ -18,10 +18,12 @@ before_install:
   - if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/before_install_linux.sh' ; fi
   - if [ "$TRAVIS_OS_NAME" = "osx"   ]; then 'ci/before_install_osx.sh'   ; fi
 install: 
-  - cmake -DBUILD_CORE_ONLY=$BUILD_CORE_ONLY -DCMAKE_BUILD_TYPE=$BUILD_TYPE .
+  - mkdir build
+  - cd build 
+  - cmake -DBUILD_CORE_ONLY=$BUILD_CORE_ONLY -DCMAKE_BUILD_TYPE=$BUILD_TYPE ..
 script:
-  - if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/script_linux.sh' ; fi
-  - if [ "$TRAVIS_OS_NAME" = "osx"   ]; then 'ci/script_osx.sh'   ; fi
+  - if [ "$TRAVIS_OS_NAME" = "linux" ]; then '../ci/script_linux.sh' ; fi
+  - if [ "$TRAVIS_OS_NAME" = "osx"   ]; then '../ci/script_osx.sh'   ; fi
 after_failure:
-  - if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/after_failure_linux.sh' ; fi
-  - if [ "$TRAVIS_OS_NAME" = "osx"   ]; then 'ci/after_failure_osx.sh'   ; fi
+  - if [ "$TRAVIS_OS_NAME" = "linux" ]; then '../ci/after_failure_linux.sh' ; fi
+  - if [ "$TRAVIS_OS_NAME" = "osx"   ]; then '../ci/after_failure_osx.sh'   ; fi

apps/CMakeLists.txt

@@ -1,29 +1,30 @@
 # A list of applications
 set_property(DIRECTORY PROPERTY FOLDER Apps)
 
+message(STATUS "")
+message(STATUS "[ Applications ]")
+
+# Automatically identify all directories in the apps folder
+file(GLOB children RELATIVE ${CMAKE_CURRENT_LIST_DIR} "*")
+set(directories "")
+foreach(child ${children})
+  if(IS_DIRECTORY "${CMAKE_CURRENT_LIST_DIR}/${child}")
+    if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/${child}/CMakeLists.txt")
+      message(STATUS "Adding application: " ${child})
+      list(APPEND directories ${child})
+    endif(EXISTS "${CMAKE_CURRENT_LIST_DIR}/${child}/CMakeLists.txt")
+  endif(IS_DIRECTORY "${CMAKE_CURRENT_LIST_DIR}/${child}")
+endforeach(child)
+
 # List of all the subdirectories to include
-foreach(APPDIR
-    addDeleteSkels
-    atlasSimbicon
-    bipedStand
-    hardcodedDesign    
-    hybridDynamics
-    jointConstraints
-    mixedChain
-    operationalSpaceControl
-    rigidChain
-    rigidCubes
-    rigidLoop    
-    softBodies
-    vehicle
-    )
-    add_subdirectory(${APPDIR})
-    if(WIN32)
-        if(TARGET ${APPTARGET})
-            set_target_properties(${APPTARGET} PROPERTIES FOLDER Apps
-                    #EXCLUDE_FROM_DEFAULT_BUILD ON
-                    )
-            set_target_properties(${APPTARGET} PROPERTIES STATIC_LIBRARY_FLAGS_RELEASE "/LTCG")
-        endif(TARGET ${APPTARGET})
-    endif(WIN32)
+foreach(APPDIR ${directories})
+  add_subdirectory(${APPDIR})
+  if(WIN32)
+    if(TARGET ${APPTARGET})
+      set_target_properties(${APPTARGET} PROPERTIES FOLDER Apps
+          #EXCLUDE_FROM_DEFAULT_BUILD ON
+          )
+      set_target_properties(${APPTARGET} PROPERTIES STATIC_LIBRARY_FLAGS_RELEASE "/LTCG")
+    endif(TARGET ${APPTARGET})
+  endif(WIN32)
 endforeach(APPDIR)

apps/atlasSimbicon/State.cpp

@@ -220,13 +220,13 @@ void State::end(double _currentTime)
 //==============================================================================
 Eigen::Vector3d State::getCOM() const
 {
-  return mSkeleton->getWorldCOM();
+  return mSkeleton->getCOM();
 }
 
 //==============================================================================
 Eigen::Vector3d State::getCOMVelocity() const
 {
-  return mSkeleton->getWorldCOMVelocity();
+  return mSkeleton->getCOMLinearVelocity();
 }
 
 //==============================================================================

apps/bipedStand/Controller.cpp

@@ -96,7 +96,7 @@ void Controller::computeTorques(const Eigen::VectorXd& _dof,
   mTorques = p + d - mKd * qddot * mTimestep;
 
   // ankle strategy for sagital plane
-  Eigen::Vector3d com = mSkel->getWorldCOM();
+  Eigen::Vector3d com = mSkel->getCOM();
   Eigen::Vector3d cop = mSkel->getBodyNode("h_heel_left")->getTransform()
                         * Eigen::Vector3d(0.05, 0, 0);
   Eigen::Vector2d diff(com[0] - cop[0], com[2] - cop[2]);

apps/bipedStand/MyWindow.cpp

@@ -154,7 +154,7 @@ void MyWindow::plotCOMX() {
     Eigen::VectorXd pose = mWorld->getRecording()->getConfig(i, 1);
     mWorld->getSkeleton(1)->setPositions(pose);
     mWorld->getSkeleton(1)->computeForwardKinematics(true, true, false);    
-    data[i] = mWorld->getSkeleton(1)->getWorldCOM()[0];
+    data[i] = mWorld->getSkeleton(1)->getCOM()[0];
   }
   if (nFrame != 0)
   {

apps/jointConstraints/Controller.cpp

@@ -93,7 +93,7 @@ void Controller::computeTorques(const Eigen::VectorXd& _dof,
   mTorques = p + d - mKd * qddot * mTimestep;
 
   // ankle strategy for sagital plane
-  Eigen::Vector3d com = mSkel->getWorldCOM();
+  Eigen::Vector3d com = mSkel->getCOM();
   Eigen::Vector3d cop = mSkel->getBodyNode("h_heel_left")->getTransform()
                         * Eigen::Vector3d(0.05, 0, 0);
   Eigen::Vector2d diff(com[0] - cop[0], com[2] - cop[2]);

apps/operationalSpaceControl/Controller.cpp

@@ -35,6 +35,7 @@
  */
 
 #include "apps/operationalSpaceControl/Controller.h"
+#include "dart/math/MathTypes.h"
 
 //==============================================================================
 Controller::Controller(dart::dynamics::Skeleton* _robot,
@@ -75,14 +76,16 @@ Controller::~Controller()
 //==============================================================================
 void Controller::update(const Eigen::Vector3d& _targetPosition)
 {
+  using namespace dart;
+
   // Get equation of motions
   Eigen::Vector3d x    = mEndEffector->getTransform().translation();
-  Eigen::Vector3d dx   = mEndEffector->getWorldLinearVelocity();
+  Eigen::Vector3d dx   = mEndEffector->getLinearVelocity();
   Eigen::MatrixXd invM = mRobot->getInvMassMatrix();                   // n x n
   Eigen::VectorXd Cg   = mRobot->getCoriolisAndGravityForces();        // n x 1
-  Eigen::MatrixXd Jv   = mEndEffector->getWorldLinearJacobian();       // 3 x n
-  Eigen::MatrixXd dJv  = mEndEffector->getWorldLinearJacobianDeriv();  // 3 x n
-  Eigen::VectorXd dq   = mRobot->getVelocities();                      // n x 1
+  math::LinearJacobian Jv   = mEndEffector->getLinearJacobian();       // 3 x n
+  math::LinearJacobian dJv  = mEndEffector->getLinearJacobianDeriv();  // 3 x n
+  Eigen::VectorXd dq        = mRobot->getVelocities();                 // n x 1
 
   // Compute operational space values
   Eigen::MatrixXd A = Jv*invM;                 // 3 x n

apps/simpleFrames/CMakeLists.txt

@@ -0,0 +1,8 @@
+###############################################
+# apps/simpleFrames
+set(APP_NAME simpleFrames)
+file(GLOB ${APP_NAME}_srcs "*.cpp")
+file(GLOB ${APP_NAME}_hdrs "*.h")
+add_executable(${APP_NAME} ${${APP_NAME}_srcs} ${${APP_NAME}_hdrs})
+target_link_libraries(${APP_NAME} dart)
+set_target_properties(${APP_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin")

apps/simpleFrames/Main.cpp

@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2015, Georgia Tech Research Corporation
+ * All rights reserved.
+ *
+ * Author(s): Michael X. Grey <mxgrey@gatech.edu>
+ *
+ * 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 "dart/dynamics/SimpleFrame.h"
+#include "dart/simulation/World.h"
+#include "dart/gui/SimWindow.h"
+
+#include "dart/dynamics/BoxShape.h"
+#include "dart/dynamics/EllipsoidShape.h"
+
+using namespace dart::dynamics;
+
+int main(int argc, char* argv[])
+{
+  dart::simulation::World myWorld;
+
+  Eigen::Isometry3d tf1(Eigen::Isometry3d::Identity());
+  tf1.translate(Eigen::Vector3d(0.1,-0.1,0));
+
+  Eigen::Isometry3d tf2(Eigen::Isometry3d::Identity());
+  tf2.translate(Eigen::Vector3d(0,0.1,0));
+  tf2.rotate(Eigen::AngleAxisd(45.0*M_PI/180.0, Eigen::Vector3d(1,0,0)));
+
+  Eigen::Isometry3d tf3(Eigen::Isometry3d::Identity());
+  tf3.translate(Eigen::Vector3d(0,0,0.1));
+  tf3.rotate(Eigen::AngleAxisd(60*M_PI/180.0, Eigen::Vector3d(0,1,0)));
+
+  SimpleFrame F1(Frame::World(), "F1", tf1);
+  F1.addVisualizationShape(new BoxShape(Eigen::Vector3d(0.05, 0.05, 0.02)));
+  SimpleFrame F2(&F1, "F2", tf2);
+  F2.addVisualizationShape(new BoxShape(Eigen::Vector3d(0.05, 0.05, 0.02)));
+  SimpleFrame F3(&F2, "F3", tf3);
+  F3.addVisualizationShape(new BoxShape(Eigen::Vector3d(0.05, 0.05, 0.02)));
+
+  // Note: Adding a Frame to the world will also cause all Entities that descend
+  // from that Frame to be rendered, but they will not be counted in the list of
+  // Entities in myWorld. So F2 and F3 will be rendered by simply adding the F1
+  // Frame
+  myWorld.addEntity(&F1);
+
+  SimpleFrame A(Frame::World(), "A");
+  A.addVisualizationShape(new EllipsoidShape(Eigen::Vector3d(0.02,0.02,0.02)));
+  SimpleFrame A1(&A, "A1", F1.getTransform(&A));
+  A1.addVisualizationShape(new EllipsoidShape(Eigen::Vector3d(0.01,0.01,0.01)));
+  SimpleFrame A2(&A, "A2", F2.getTransform(&A));
+  A2.addVisualizationShape(new EllipsoidShape(Eigen::Vector3d(0.01,0.01,0.01)));
+  SimpleFrame A3(&A, "A3", F3.getTransform(&A));
+  A3.addVisualizationShape(new EllipsoidShape(Eigen::Vector3d(0.01,0.01,0.01)));
+
+  myWorld.addEntity(&A);
+
+  // CAREFUL: For an Entity (or Frame) that gets added to the world to be
+  // rendered correctly, it must be a child of the World Frame
+  // TODO(MXG): Fix this issue ^
+
+  dart::gui::SimWindow window;
+  window.setWorld(&myWorld);
+
+  glutInit(&argc, argv);
+  window.initWindow(640, 480, "Simple Frames");
+  glutMainLoop();
+}

apps/speedTest/CMakeLists.txt

@@ -0,0 +1,10 @@
+###############################################################
+# This file can be used as-is in the directory of any app,    #
+# however you might need to specify your own dependencies in  #
+# target_link_libraries if your app depends on more than dart #
+###############################################################
+get_filename_component(app_name ${CMAKE_CURRENT_LIST_DIR} NAME)
+file(GLOB ${app_name}_srcs "*.cpp" "*.h" "*.hpp")
+add_executable(${app_name} ${${app_name}_srcs})
+target_link_libraries(${app_name} dart)
+set_target_properties(${app_name} PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin")