Commit bd096576 authored by fvalenza's avatar fvalenza
Browse files

Merge pull request #17 from jcarpent/topic/lua-parser

Add LUA parser
parents f96a04b7 5c782b76
......@@ -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()
......@@ -78,7 +79,11 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint-free-flyer.hpp
multibody/joint/joint-variant.hpp
multibody/joint/joint-generic.hpp
)
)
SET(${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
multibody/parser/sample-models.hpp
)
SET(${PROJECT_NAME}_MULTIBODY_HEADERS
multibody/constraint.hpp
......@@ -86,7 +91,6 @@ SET(${PROJECT_NAME}_MULTIBODY_HEADERS
multibody/joint.hpp
multibody/model.hpp
multibody/visitor.hpp
multibody/parser/sample-models.hpp
)
SET(${PROJECT_NAME}_ALGORITHM_HEADERS
......@@ -122,18 +126,35 @@ SET(HEADERS
${${PROJECT_NAME}_TOOLS_HEADERS}
${${PROJECT_NAME}_SPATIAL_HEADERS}
${${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS}
${${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS}
${${PROJECT_NAME}_MULTIBODY_HEADERS}
${${PROJECT_NAME}_ALGORITHM_HEADERS}
${${PROJECT_NAME}_SIMULATION_HEADERS}
${${PROJECT_NAME}_PYTHON_HEADERS}
exception.hpp
assert.hpp
)
)
IF(URDFDOM_FOUND)
LIST(APPEND HEADERS multibody/parser/urdf.hpp)
SET(${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
${${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS}
multibody/parser/urdf.hpp
)
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")
......@@ -142,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")
......
inertia = {
{1.1, 0.1, 0.2},
{0.1, 1.2, 0.4},
{0.2, 0.4, 1.3}
}
pelvis = { mass = 9.3, com = { 1.1, 1.2, 1.3}, inertia = inertia }
thigh = { mass = 4.2, com = { 1.1, 1.2, 1.3}, inertia = inertia }
shank = { mass = 4.1, com = { 1.1, 1.2, 1.3}, inertia = inertia }
foot = { mass = 1.1, com = { 1.1, 1.2, 1.3}, inertia = inertia }
bodies = {
pelvis = pelvis,
thigh_right = thigh,
shank_right = shank,
thigh_left = thigh,
shank_left = shank
}
joints = {
freeflyer = {
'JointTypeFloatingBase'
},
spherical_zyx = {
'JointTypeEulerZYX'
},
rotational_y = {
'JointTypeRevoluteY'
},
fixed = {},
}
model = {
frames = {
{
name = "pelvis",
--parent = "ROOT",
body = bodies.pelvis,
--joint = joints.freeflyer,
},
{
name = "thigh_right",
parent = "pelvis",
body = bodies.thigh_right,
joint = joints.spherical_zyx,
joint_name = "hip_right",
},
{
name = "shank_right",
parent = "thigh_right",
body = bodies.thigh_right,
joint = joints.rotational_y
},
{
name = "foot_right",
parent = "shank_right",
body = bodies.thigh_right,
joint = joints.fixed
},
{
name = "thigh_left",
parent = "pelvis",
body = bodies.thigh_left,
joint = joints.spherical_zyx,
joint_name = "hip_left",
},
{
name = "shank_left",
parent = "thigh_left",
body = bodies.thigh_left,
joint = joints.rotational_y
},
{
name = "foot_left",
parent = "shank_left",
body = bodies.thigh_left,
joint = joints.fixed
},
}
}
return model
MACRO (ADD_HEADERS_GROUP NAME LIST_OF_FILES)
SOURCE_GROUP ( "Header Files\\${NAME}" FILES ${${LIST_OF_FILES}} )
ENDMACRO (ADD_HEADERS_GROUP)
MACRO(ADD_GROUP GROUP_NAME FILENAMES)
FOREACH(filename ${${FILENAMES}})
GET_FILENAME_COMPONENT(filenamePath ${filename} PATH)
IF(NOT (filenamePath STREQUAL ""))
STRING(REGEX REPLACE "/" "\\\\" filenamePath ${filenamePath})
SOURCE_GROUP("${GROUP_NAME}\\${filenamePath}" FILES ${filename})
ELSE()
SOURCE_GROUP("${GROUP_NAME}" FILES ${filename})
ENDIF()
ENDFOREACH()
ENDMACRO(ADD_GROUP)
MACRO (ADD_HEADERS_SUB_GROUP GROUP SUB_GROUP LIST_OF_FILES)
SOURCE_GROUP ( "Header Files\\${GROUP}\\${SUB_GROUP}" FILES ${${LIST_OF_FILES}} )
ENDMACRO (ADD_HEADERS_SUB_GROUP)
MACRO(ADD_HEADER_GROUP FILENAMES)
ADD_GROUP("Header Files" ${FILENAMES})
ENDMACRO(ADD_HEADER_GROUP FILENAMES)
MACRO (ADD_SOURCES_GROUP NAME LIST_OF_FILES)
SOURCE_GROUP ( "Source Files\\${NAME}" FILES ${${LIST_OF_FILES}} )
ENDMACRO (ADD_SOURCES_GROUP)
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 -----------------------------------------
......@@ -24,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
......@@ -72,23 +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_HEADERS_GROUP ("math" ${PROJECT_NAME}_MATH_HEADERS )
# Install target libpinocchio.so
INSTALL(FILES
"${CMAKE_BINARY_DIR}/src/lib${PROJECT_NAME}.so"
DESTINATION ${CMAKE_INSTALL_LIBDIR})
ADD_HEADERS_GROUP ("tools" ${PROJECT_NAME}_TOOLS_HEADERS )
ADD_HEADERS_GROUP ("spatial" ${PROJECT_NAME}_SPATIAL_HEADERS )
ADD_HEADERS_GROUP ("multibody" ${PROJECT_NAME}_MULTIBODY_HEADERS )
ADD_HEADERS_SUB_GROUP ("multibody" "joint" ${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS )
ADD_HEADERS_GROUP ("algorithm" ${PROJECT_NAME}_ALGORITHM_HEADERS )
ADD_HEADERS_GROUP ("python" ${PROJECT_NAME}_PYTHON_HEADERS )
ENDIF ( )
#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() == 0)
joint_type = "JointTypeFixed";
else if (model_table["frames"][i]["joint"].length() == 1)
joint_type = model_table["frames"][i]["joint"][1].getDefault<std::string> ("");
else
{
joint_type = model_table["frames"][i]["joint"][1].getDefault<std::string> ("");
std::cerr << "Joint compouned not yet implemented. Take only the first joint." << std::endl;
}
}
else if (i == 1) // first body of the tree
{
if (freeFlyer)
joint_type = "JointTypeFloatbase";
else
{
std::cerr << "The first segment is defined without any definition of joint type relatily to the world." << 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 == "JointTypeRevoluteX")
{
body_id = model.addBody (parent_id, JointModelRX (), global_placement, Y, joint_name, body_name, false);
}
else if (joint_type == "JointTypeRevoluteY")
{
body_id = model.addBody (parent_id, JointModelRY (), global_placement, Y, joint_name, body_name, false);
}
else if (joint_type == "JointTypeRevoluteZ")
{
body_id = model.addBody (parent_id, JointModelRZ (), global_placement, Y, joint_name, body_name, false);
}
else if (joint_type == "JointTypePrismaticX")
{
body_id = model.addBody (parent_id, JointModelPX (), global_placement, Y, joint_name, body_name, false);
}
else if (joint_type == "JointTypePrismaticY")
{
body_id = model.addBody (parent_id, JointModelPY (), global_placement, Y, joint_name, body_name, false);
}
else if (joint_type == "JointTypePrismaticZ")
{
body_id = model.addBody (parent_id, JointModelPZ (), global_placement, Y, joint_name, body_name, false);
}
else if (joint_type == "JointTypeFloatingBase" || joint_type == "JointTypeFloatbase")
{
body_id = model.addBody (parent_id, JointModelFreeFlyer (), global_placement, Y, joint_name, body_name, false);
}
else if (joint_type == "JointTypeSpherical")
{
body_id = model.addBody (parent_id, JointModelSpherical (), global_placement, Y, joint_name, body_name, false);
}
else if (joint_type == "JointTypeEulerZYX")
{
body_id = model.addBody (parent_id, JointModelSphericalZYX (), global_placement, Y, joint_name, body_name, false);
}
else if (joint_type == "JointTypeFixed")
{
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
#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__
This diff is collapsed.
/*
* 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.
*