[Incremental][C++] Add Lua parser

CMakeLists.txt

@@ -37,6 +37,7 @@ ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.0.5")
 ADD_OPTIONAL_DEPENDENCY("eigenpy >= 1.2.0")
 ADD_OPTIONAL_DEPENDENCY("metapod >= 1.0.7")
 ADD_OPTIONAL_DEPENDENCY("urdfdom >= 0.2.10")
+ADD_OPTIONAL_DEPENDENCY("lua5.1")
 
 SET(BOOST_COMPONENTS filesystem unit_test_framework)
 SEARCH_FOR_BOOST()
@@ -142,6 +143,18 @@ IF(URDFDOM_FOUND)
     )
 ENDIF(URDFDOM_FOUND)
 
+IF(LUA5_1_FOUND)
+  LIST(APPEND HEADERS 
+    multibody/parser/lua.hpp
+    multibody/parser/lua/lua_tables.hpp
+    )
+
+  SET(${PROJECT_NAME}_MULTIBODY_PARSER_LUA_HEADERS
+    multibody/parser/lua.hpp
+    multibody/parser/lua/lua_tables.hpp
+    )
+ENDIF(LUA5_1_FOUND)
+
 PKG_CONFIG_APPEND_LIBS (${PROJECT_NAME})
 
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio")
@@ -150,6 +163,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/spatial")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/joint")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/parser")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/parser/lua")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/tools")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/simulation")

src/CMakeLists.txt

@@ -18,6 +18,36 @@ MACRO(ADD_SOURCE_GROUP FILENAMES)
   ADD_GROUP("Source Files" ${FILENAMES})
 ENDMACRO(ADD_SOURCE_GROUP FILENAMES)
 
+# ----------------------------------------------------
+# --- C++ --------------------------------------------
+# ----------------------------------------------------
+
+IF(LUA5_1_FOUND)
+  SET(${PROJECT_NAME}_MULTIBODY_PARSER_LUA_SOURCES 
+      multibody/parser/lua/lua_tables.cpp
+      multibody/parser/lua.cpp
+    )
+ENDIF(LUA5_1_FOUND)
+
+SET(${PROJECT_NAME}_SOURCES ${${PROJECT_NAME}_MULTIBODY_PARSER_LUA_SOURCES}) 
+
+IF (UNIX )  
+    # Create target libpinocchio.so
+    ADD_LIBRARY ( ${PROJECT_NAME} SHARED ${HEADERS} ${${PROJECT_NAME}_SOURCES} )
+		SET_TARGET_PROPERTIES( ${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX)
+
+    IF(LUA5_1_FOUND)
+      PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} lua5.1)
+    ENDIF(LUA5_1_FOUND)
+    
+    ADD_HEADER_GROUP(HEADERS)  
+    ADD_SOURCE_GROUP(${PROJECT_NAME}_SOURCES)  
+
+    # Install target libpinocchio.so
+    INSTALL(FILES
+      "${CMAKE_BINARY_DIR}/src/lib${PROJECT_NAME}.so"
+      DESTINATION ${CMAKE_INSTALL_LIBDIR})
+ENDIF ( )
 
 # ----------------------------------------------------
 # --- PYTHON -----------------------------------------
@@ -33,9 +63,11 @@ IF(EIGENPY_FOUND)
   PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} eigenpy)
   IF(URDFDOM_FOUND)
     PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} urdfdom)
-      
   ENDIF(URDFDOM_FOUND)
-  TARGET_LINK_LIBRARIES(${PYWRAP} ${Boost_LIBRARIES} eigenpy)
+  IF(LUA5_1_FOUND)
+    PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} lua5.1)
+  ENDIF(LUA5_1_FOUND)
+  TARGET_LINK_LIBRARIES(${PYWRAP} ${Boost_LIBRARIES} ${PROJECT_NAME})
   SET_TARGET_PROPERTIES(${PYWRAP} PROPERTIES
     LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib/python/${PROJECT_NAME}")
   INSTALL(FILES
@@ -81,17 +113,3 @@ IF(EIGENPY_FOUND)
 
 ENDIF(EIGENPY_FOUND)
 
-IF (UNIX )  
-    # Create target libpinocchio.so
-    ADD_LIBRARY ( ${PROJECT_NAME} SHARED ${HEADERS} ${${PROJECT_NAME}_SOURCES} )
-		SET_TARGET_PROPERTIES( ${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX)
-    
-		ADD_HEADER_GROUP(HEADERS)  
-		ADD_SOURCE_GROUP(${PROJECT_NAME}_SOURCES)  
-
-    # Install target libpinocchio.so
-    INSTALL(FILES
-      "${CMAKE_BINARY_DIR}/src/lib${PROJECT_NAME}.so"
-      DESTINATION ${CMAKE_INSTALL_LIBDIR})
-
-ENDIF ( )

src/multibody/parser/lua.cpp

+
+#include "pinocchio/multibody/parser/lua/lua_tables.hpp"
+#include "pinocchio/multibody/parser/lua.hpp"
+
+#include <lua.hpp>
+#include <iostream>
+#include <map>
+#include <sstream>
+
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/spatial/motion.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/multibody/model.hpp"
+
+typedef se3::SE3::Vector3 Vector3;
+typedef se3::SE3::Matrix3 Matrix3;
+
+template<> Vector3 LuaTableNode::getDefault<Vector3> (const Vector3 & default_value)
+{
+  Vector3 result (default_value);
+
+  if (stackQueryValue()) {
+    LuaTable vector_table = LuaTable::fromLuaState (luaTable->L);
+
+    if (vector_table.length() != 3) {
+      std::cerr << "LuaModel Error: invalid 3d vector!" << std::endl;
+      abort();
+    }
+
+    result[0] = vector_table[1];
+    result[1] = vector_table[2];
+    result[2] = vector_table[3];
+  }
+
+  stackRestore();
+
+  return result;
+}
+
+template<> Matrix3 LuaTableNode::getDefault<Matrix3> (const Matrix3 & default_value)
+{
+  Matrix3 result (default_value);
+
+  if (stackQueryValue()) {
+    LuaTable vector_table = LuaTable::fromLuaState (luaTable->L);
+
+    if (vector_table.length() != 3) {
+      std::cerr << "LuaModel Error: invalid 3d matrix!" << std::endl;
+      abort();
+    }
+
+    if (vector_table[1].length() != 3
+        || vector_table[2].length() != 3
+        || vector_table[3].length() != 3) {
+      std::cerr << "LuaModel Error: invalid 3d matrix!" << std::endl;
+      abort();
+    }
+
+    result(0,0) = vector_table[1][1];
+    result(0,1) = vector_table[1][2];
+    result(0,2) = vector_table[1][3];
+
+    result(1,0) = vector_table[2][1];
+    result(1,1) = vector_table[2][2];
+    result(1,2) = vector_table[2][3];
+
+    result(2,0) = vector_table[3][1];
+    result(2,1) = vector_table[3][2];
+    result(2,2) = vector_table[3][3];
+  }
+
+  stackRestore();
+
+  return result;
+}
+
+template<> se3::SE3 LuaTableNode::getDefault<se3::SE3> (const se3::SE3 & default_value)
+{
+  se3::SE3 result (default_value);
+
+  if (stackQueryValue()) {
+    LuaTable vector_table = LuaTable::fromLuaState (luaTable->L);
+
+    result.translation() = vector_table["r"].getDefault<Vector3> (Vector3::Zero (3));
+    result.rotation().transpose() = vector_table["E"].getDefault<Matrix3> (Matrix3::Identity (3,3));
+  }
+
+  stackRestore();
+
+  return result;
+}
+
+template<> se3::Inertia LuaTableNode::getDefault<se3::Inertia> (const se3::Inertia & default_value)
+{
+  se3::Inertia result (default_value);
+
+  if (stackQueryValue()) {
+    LuaTable vector_table = LuaTable::fromLuaState (luaTable->L);
+
+    double mass = 0.;
+    Vector3 com;
+    Matrix3 inertia_matrix;
+
+    mass = vector_table["mass"];
+    com = vector_table["com"].getDefault<Vector3> (Vector3::Zero ());
+    inertia_matrix = vector_table["inertia"].getDefault<Matrix3> (Matrix3::Identity ());
+
+    result = se3::Inertia (mass, com, inertia_matrix);
+  }
+
+  stackRestore();
+
+  return result;
+}
+
+namespace se3
+{
+  namespace lua
+  {
+    bool LuaModelReadFromTable (LuaTable & model_table, Model & model, bool freeFlyer, bool verbose)
+    {
+      typedef std::map<std::string, Model::Index> mapStringIndex_t;
+      mapStringIndex_t body_table_id_map;
+      mapStringIndex_t fixed_body_table_id_map;
+
+      typedef std::map<std::string, SE3> mapStringSE3_t;
+      mapStringSE3_t fixed_placement_map;
+
+      if (model_table["gravity"].exists())
+      {
+        model.gravity.linear() = model_table["gravity"].get<Vector3> ();
+
+        if (verbose)
+          std::cout << "gravity = " << model.gravity.linear().transpose() << std::endl;
+      }
+
+      if (! model_table["frames"].exists())
+      {
+        std::cerr << "Frames table missing from model table - Abort" << std::endl;
+        abort();
+      }
+
+      size_t frame_count = model_table["frames"].length();
+
+      body_table_id_map["ROOT"] = 0;
+
+      for (int i = 1; i <= (int) frame_count; i++) {
+
+        std::stringstream body_name_default;
+        body_name_default << "body " << i;
+        std::string body_name = model_table["frames"][i]["name"].getDefault<std::string> (body_name_default.str());
+
+        std::string parent_name;
+        if (model_table["frames"][i]["parent"].exists ())
+        {
+          parent_name = model_table["frames"][i]["parent"].get<std::string> ();
+        }
+        else if (i == 1) // first body of the tree
+        {
+          parent_name = "ROOT";
+        }
+        else // no parent defined
+        {
+          std::cerr << "Parent not defined for frame " << i << "." << std::endl;
+          abort();
+        }
+
+        Model::Index parent_id;
+        SE3 fixed_placement_offset (SE3::Identity());;
+
+        if (body_table_id_map.find (parent_name) != body_table_id_map.end ())
+        {
+          parent_id = body_table_id_map[parent_name];
+        }
+        else if (fixed_body_table_id_map.find(parent_name) != fixed_body_table_id_map.end ()) // the parent is fixed in the kinematic tree
+        {
+          parent_id = fixed_body_table_id_map[parent_name];
+          fixed_placement_offset = fixed_placement_map[parent_name];
+        }
+        else
+        {
+          std::cerr << parent_name << " is not in the tree." << std::endl;
+          abort ();
+        }
+
+
+        std::stringstream joint_name_default;
+        joint_name_default << "joint " << i;
+        std::string joint_name = model_table["frames"][i]["joint_name"].getDefault<std::string> (joint_name_default.str());
+
+        
+        SE3 joint_placement = model_table["frames"][i]["joint_frame"].getDefault<SE3> (SE3::Identity ());
+        SE3 global_placement (fixed_placement_offset * joint_placement); // placement due to the existence of fixed bodies
+
+        if (! model_table["frames"][i]["body"].exists()) {
+          std::cerr << "body field not defined for frame " << i << "." << std::endl;
+          abort();
+        }
+
+        Inertia Y = model_table["frames"][i]["body"].getDefault<Inertia> (Inertia::Identity ());
+
+        std::string joint_type;
+        if (model_table["frames"][i]["joint"].exists())
+        {
+          if (model_table["frames"][i]["joint"].length() == 1)
+            joint_type = model_table["frames"][i]["joint"][1].getDefault<std::string> ("");
+          else
+            std::cerr << "Joint compouned not implemented yet. Take only the first joint." << std::endl;
+        }
+        else if (i == 1) // first body of the tree
+        {
+          if (freeFlyer)
+            joint_type = "jointFreeFlyer";
+          else
+          {
+            std::cerr << "The first segment is defined without any definition of joint type relatily to the environment." << std::endl;
+            abort ();
+          }
+        }
+        else
+        {
+          std::cerr << "joint field not defined for frame " << i << "." << std::endl;
+          abort();
+        }
+
+
+        Model::Index body_id;
+        bool is_body_fixed = false;
+        if (joint_type == "jointRX")
+        {
+          body_id = model.addBody (parent_id, JointModelRX (), global_placement, Y, joint_name, body_name, false);
+        }
+        else if (joint_type == "jointRY")
+        {
+          body_id = model.addBody (parent_id, JointModelRY (), global_placement, Y, joint_name, body_name, false);
+        }
+        else if (joint_type == "jointRZ")
+        {
+          body_id = model.addBody (parent_id, JointModelRZ (), global_placement, Y, joint_name, body_name, false);
+        }
+        else if (joint_type == "jointPX")
+        {
+          body_id = model.addBody (parent_id, JointModelPX (), global_placement, Y, joint_name, body_name, false);
+        }
+        else if (joint_type == "jointPY")
+        {
+          body_id = model.addBody (parent_id, JointModelPY (), global_placement, Y, joint_name, body_name, false);
+        }
+        else if (joint_type == "jointPZ")
+        {
+          body_id = model.addBody (parent_id, JointModelPZ (), global_placement, Y, joint_name, body_name, false);
+        }
+        else if (joint_type == "jointFreeFlyer")
+        {
+          body_id = model.addBody (parent_id, JointModelFreeFlyer (), global_placement, Y, joint_name, body_name, false);
+        }
+        else if (joint_type == "jointSpherical")
+        {
+          body_id = model.addBody (parent_id, JointModelSpherical (), global_placement, Y, joint_name, body_name, false);
+        }
+        else if (joint_type == "jointSphericalZYX")
+        {
+          body_id = model.addBody (parent_id, JointModelSphericalZYX (), global_placement, Y, joint_name, body_name, false);
+        }
+        else if (joint_type == "jointFixed")
+        {
+          model.mergeFixedBody(parent_id, global_placement, Y);
+
+          fixed_body_table_id_map[body_name] = parent_id;
+          fixed_placement_map[body_name] = global_placement;
+
+          body_id = model.addFixedBody(parent_id, global_placement, body_name, false);
+          is_body_fixed = true;
+        }
+        else
+        {
+          std::cerr << joint_type << " is not supported.." << std::endl;
+          abort ();
+        }
+
+        body_table_id_map[body_name] = body_id;
+
+        if (verbose) {
+          std::cout << "==== Added Body ====" << std::endl;
+          std::cout << "  body name  : " << body_name << std::endl;
+          if (! is_body_fixed)
+            std::cout << "  body id    : " << body_id << std::endl;
+          else
+            std::cout << "  fixed body id    : " << body_id << std::endl;
+          std::cout << "  closest parent id  : " << parent_id << std::endl;
+          std::cout << "  joint frame:\n" << joint_placement << std::endl;
+          std::cout << "  joint type : " << joint_type << std::endl;
+          std::cout << "  joint name : " << joint_name << std::endl;
+          std::cout << "  body inertia:\n" << Y << std::endl;
+        }
+      }
+      
+      return true;
+    }
+
+    Model buildModel (const std::string & filename, bool freeFlyer, bool verbose)
+    {
+      Model model;
+
+      LuaTable model_table = LuaTable::fromFile (filename.c_str ());
+      LuaModelReadFromTable (model_table, model, freeFlyer, verbose);
+
+      return model;
+    }
+
+  } // namespace lua
+
+} // namespace se3

src/multibody/parser/lua.hpp

+#ifndef __se3_lua_hpp__
+#define __se3_lua_hpp__
+
+#include <iostream>
+#include "pinocchio/multibody/model.hpp"
+
+namespace se3
+{
+  namespace lua
+  {
+    Model buildModel (const std::string & filename, bool freeFlyer = false, bool verbose = false);
+  } // namespace lua
+} // namespace se3
+
+#endif // ifndef __se3_lua_hpp__

src/multibody/parser/lua/lua_tables.hpp

+/*
+ * LuaTables++
+ * Code comming from https://bitbucket.org/rbdl/rbdl
+ * Copyright (c) 2013-2014 Martin Felis <martin@fyxs.org>
+ * Copyright (c) 2015 Justin Carpentier <jcarpent@laas.fr>
+ * All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef __se3_parser_lua_tables_hpp____
+#define __se3_parser_lua_tables_hpp____
+
+#include <iostream>
+#include <assert.h>
+#include <cstdlib>
+#include <string>
+#include <vector>
+
+
+// Forward declaration for Lua
+extern "C" {
+  struct lua_State;
+}
+
+struct LuaKey {
+  enum Type {
+    String,
+    Integer
+  };
+
+  Type type;
+  int int_value;
+  std::string string_value;
+
+  bool operator<( const LuaKey & rhs ) const {
+    if (type == String && rhs.type == Integer) {
+      return false;
+    } else if (type == Integer && rhs.type == String) {
+      return true;
+    } else if (type == Integer && rhs.type == Integer) {
+      return int_value < rhs.int_value;
+    }
+
+    return string_value < rhs.string_value;
+  }
+
+  LuaKey (const char* key_value) :
+  type (String),
+  int_value (0),
+  string_value (key_value) { }
+  LuaKey (int key_value) :
+  type (Integer),
+  int_value (key_value),
+  string_value ("") {}
+};
+
+inline std::ostream & operator<<(std::ostream & output, const LuaKey & key) {
+  if (key.type == LuaKey::Integer)
+    output << key.int_value << "(int)";
+  else
+    output << key.string_value << "(string)";
+  return output;
+}
+struct LuaTable;
+struct LuaTableNode;
+
+struct LuaTableNode {
+  LuaTableNode() :
+  parent (NULL),
+  luaTable (NULL),
+  key("")
+  { }
+  LuaTableNode operator[](const char* child_str) {
+    LuaTableNode child_node;
+
+    child_node.luaTable = luaTable;
+    child_node.parent = this;
+    child_node.key = LuaKey (child_str);
+
+    return child_node;
+  }
+  LuaTableNode operator[](int child_index) {
+    LuaTableNode child_node;
+
+    child_node.luaTable = luaTable;
+    child_node.parent = this;
+    child_node.key = LuaKey (child_index);
+
+    return child_node;
+  }
+  bool stackQueryValue();
+  void stackPushKey();
+  void stackCreateValue();
+  void stackRestore();
+  LuaTable stackQueryTable();
+  LuaTable stackCreateLuaTable();
+
+  std::vector<LuaKey> getKeyStack();
+  std::string keyStackToString();
+
+  bool exists();
+  void remove();
+  size_t length();
+  std::vector<LuaKey> keys();
+
+  // Templates for setters and getters. Can be specialized for custom
+  // types.
+  template <typename T>
+  void set (const T & value);
+  template <typename T>
+  T getDefault (const T & default_value);
+
+  template <typename T>
+  T get() {
+    if (!exists()) {
+      std::cerr << "Error: could not find value " << keyStackToString() << "." << std::endl;
+      abort();
+    }
+    return getDefault (T());
+  }
+
+  // convenience operators (assignment, conversion, comparison)
+  template <typename T>
+  void operator=(const T & value) {
+    set<T>(value);
+  }
+  template <typename T>
+  operator T() {
+    return get<T>();
+  }
+  template <typename T>
+  bool operator==(T value) {
+    return value == get<T>();
+  }
+  template <typename T>
+  bool operator!=(T value) {
+    return value != get<T>();
+  }
+
+  LuaTableNode *parent;
+  LuaTable *luaTable;
+  LuaKey key;
+  int stackTop;
+};
+
+template<typename T>
+bool operator==(T value, LuaTableNode node) {
+  return value == (T) node;
+}
+template<typename T>
+bool operator!=(T value, LuaTableNode node) {
+  return value != (T) node;
+}
+
+template<> bool LuaTableNode::getDefault<bool>(const bool & default_value);
+template<> double LuaTableNode::getDefault<double>(const double & default_value);