diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2b5bcd4caa77057a20c21a2a8b1ed7ff7966559e..274b8217be96a1ba7241d9f488db6f79a0441b72 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -48,6 +48,7 @@ ENDIF(WIN32)
OPTION (BUILD_BENCHMARK "Build the benchmarks" OFF)
OPTION (BUILD_UTILS "Build the utils" OFF)
OPTION (INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" OFF)
+OPTION (BUILD_TESTS_WITH_HPP "Build geom tests and benchmarks with hpp to do comparisons" OFF)
IF (INITIALIZE_WITH_NAN)
MESSAGE (STATUS "Initialize with NaN all the Eigen entries")
@@ -62,8 +63,12 @@ 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")
+ADD_OPTIONAL_DEPENDENCY("hpp-fcl")
+IF(HPP_FCL_FOUND AND URDFDOM_FOUND)
+ ADD_REQUIRED_DEPENDENCY("assimp")
+ENDIF(HPP_FCL_FOUND AND URDFDOM_FOUND)
-SET(BOOST_COMPONENTS filesystem unit_test_framework)
+SET(BOOST_COMPONENTS filesystem unit_test_framework system)
SEARCH_FOR_BOOST()
# Path to boost headers
INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
@@ -157,44 +162,68 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS
python/explog.hpp
)
-SET(HEADERS
- ${${PROJECT_NAME}_MATH_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(HPP_FCL_FOUND)
+ LIST(APPEND ${PROJECT_NAME}_PYTHON_HEADERS
+ python/geometry-model.hpp
+ python/geometry-data.hpp
+ )
+ENDIF(HPP_FCL_FOUND)
IF(URDFDOM_FOUND)
- SET(${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
- ${${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS}
+ LIST(APPEND ${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
multibody/parser/urdf.hpp
multibody/parser/urdf.hxx
)
- LIST(APPEND HEADERS
- ${${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS}
- )
+
+ IF(HPP_FCL_FOUND )
+ LIST(APPEND ${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
+ multibody/parser/from-collada-to-fcl.hpp
+ multibody/parser/urdf-with-geometry.hpp
+ multibody/parser/urdf-with-geometry.hxx
+ )
+ ENDIF(HPP_FCL_FOUND)
+
ADD_DEFINITIONS(-DWITH_URDFDOM)
ENDIF(URDFDOM_FOUND)
+IF(HPP_FCL_FOUND)
+ LIST(APPEND ${PROJECT_NAME}_SPATIAL_HEADERS
+ spatial/fcl-pinocchio-conversions.hpp
+ )
+ LIST(APPEND ${PROJECT_NAME}_MULTIBODY_HEADERS
+ multibody/geometry.hpp
+ multibody/geometry.hxx
+ )
+ LIST(APPEND ${PROJECT_NAME}_ALGORITHM_HEADERS
+ algorithm/collisions.hpp
+ )
+ENDIF(HPP_FCL_FOUND)
+
IF(LUA5_1_FOUND)
- SET(${PROJECT_NAME}_MULTIBODY_PARSER_LUA_HEADERS
+ LIST(APPEND ${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
multibody/parser/lua.hpp
multibody/parser/lua/lua_tables.hpp
)
- LIST(APPEND HEADERS
- ${${PROJECT_NAME}_MULTIBODY_PARSER_LUA_HEADERS}
- )
ADD_DEFINITIONS(-DWITH_LUA)
ENDIF(LUA5_1_FOUND)
+
+SET(HEADERS
+ ${${PROJECT_NAME}_MATH_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
+ )
+LIST(REMOVE_DUPLICATES HEADERS)
+
PKG_CONFIG_APPEND_LIBS (${PROJECT_NAME})
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio")
@@ -209,6 +238,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/simulation")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/python")
+
FOREACH(header ${HEADERS})
GET_FILENAME_COMPONENT(headerName ${header} NAME)
GET_FILENAME_COMPONENT(headerPath ${header} PATH)
diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt
index 8f12a1de669a79c4659a5a731f08ad4d00babe94..e7026c372eef46bdb976739e9bbe951f83f8c8c9 100644
--- a/benchmark/CMakeLists.txt
+++ b/benchmark/CMakeLists.txt
@@ -1,10 +1,39 @@
# ----------------------------------------------------
# --- BENCHMARK --------------------------------------
# ----------------------------------------------------
+MACRO(ADD_TEST_CFLAGS target flag)
+ SET_PROPERTY(TARGET ${target} APPEND_STRING PROPERTY COMPILE_FLAGS " ${flag}")
+ENDMACRO(ADD_TEST_CFLAGS)
+
+# timings
+#
ADD_EXECUTABLE(timings timings.cpp)
PKG_CONFIG_USE_DEPENDENCY(timings eigen3)
-PKG_CONFIG_USE_DEPENDENCY(timings urdfdom)
-TARGET_LINK_LIBRARIES(timings ${PROJECT_NAME})
-
+IF(URDFDOM_FOUND)
+ PKG_CONFIG_USE_DEPENDENCY(timings urdfdom)
+ENDIF(URDFDOM_FOUND)
+TARGET_LINK_LIBRARIES (timings ${Boost_LIBRARIES} ${PROJECT_NAME})
SET_TARGET_PROPERTIES (timings PROPERTIES COMPILE_DEFINITIONS PINOCCHIO_SOURCE_DIR="${${PROJECT_NAME}_SOURCE_DIR}")
+
+# geomTimings
+#
+IF(URDFDOM_FOUND AND HPP_FCL_FOUND)
+ ADD_EXECUTABLE(geomTimings timings-geometry.cpp)
+ PKG_CONFIG_USE_DEPENDENCY(geomTimings eigen3)
+ PKG_CONFIG_USE_DEPENDENCY(geomTimings urdfdom)
+ PKG_CONFIG_USE_DEPENDENCY(geomTimings assimp)
+ PKG_CONFIG_USE_DEPENDENCY(geomTimings hpp-fcl)
+ ADD_TEST_CFLAGS(geomTimings "-DWITH_HPP_FCL")
+ IF(BUILD_TESTS_WITH_HPP)
+ ADD_OPTIONAL_DEPENDENCY("hpp-model-urdf")
+ IF(HPP_MODEL_URDF_FOUND)
+ PKG_CONFIG_USE_DEPENDENCY(geomTimings hpp-model-urdf)
+ ADD_TEST_CFLAGS(geomTimings "-DWITH_HPP_MODEL_URDF")
+ ENDIF(HPP_MODEL_URDF_FOUND)
+ ENDIF(BUILD_TESTS_WITH_HPP)
+ TARGET_LINK_LIBRARIES (geomTimings ${Boost_LIBRARIES} ${PROJECT_NAME})
+ SET_TARGET_PROPERTIES (geomTimings PROPERTIES COMPILE_DEFINITIONS PINOCCHIO_SOURCE_DIR="${${PROJECT_NAME}_SOURCE_DIR}")
+ENDIF(URDFDOM_FOUND AND HPP_FCL_FOUND)
+
+
diff --git a/benchmark/timings-geometry.cpp b/benchmark/timings-geometry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..061692b4cf716b5a46a184cb7b6e0176f2944c13
--- /dev/null
+++ b/benchmark/timings-geometry.cpp
@@ -0,0 +1,282 @@
+//
+// Copyright (c) 2015 - 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/multibody/joint.hpp"
+#include "pinocchio/multibody/visitor.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/non-linear-effects.hpp"
+#include "pinocchio/algorithm/cholesky.hpp"
+#include "pinocchio/algorithm/jacobian.hpp"
+#include "pinocchio/algorithm/center-of-mass.hpp"
+#include "pinocchio/simulation/compute-all-terms.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/collisions.hpp"
+#include "pinocchio/multibody/parser/urdf.hpp"
+#include "pinocchio/multibody/parser/sample-models.hpp"
+
+
+#include "pinocchio/multibody/geometry.hpp"
+#include "pinocchio/multibody/parser/urdf-with-geometry.hpp"
+#ifdef WITH_HPP_MODEL_URDF
+ #include <hpp/util/debug.hh>
+ #include <hpp/model/device.hh>
+ #include <hpp/model/body.hh>
+ #include <hpp/model/collision-object.hh>
+ #include <hpp/model/joint.hh>
+ #include <hpp/model/urdf/util.hh>
+#endif
+
+#include <iostream>
+
+#include "pinocchio/tools/timer.hpp"
+
+#include <Eigen/StdVector>
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)
+
+int main()
+{
+ using namespace Eigen;
+ using namespace se3;
+
+ StackTicToc timer(StackTicToc::US);
+ #ifdef NDEBUG
+ const unsigned int NBT = 1000*100;
+ const unsigned int NBD = 1000; // for heavy tests, like computeDistances()
+ #else
+ const unsigned int NBT = 1;
+ const unsigned int NBD = 1;
+ std::cout << "(the time score in debug mode is not relevant) " << std::endl;
+ #endif
+
+
+
+ std::string romeo_filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+ std::string romeo_meshDir = PINOCCHIO_SOURCE_DIR"/models/";
+ std::pair < Model, GeometryModel > romeo = se3::urdf::buildModelAndGeom(romeo_filename, romeo_meshDir, se3::JointModelFreeFlyer());
+ se3::Model romeo_model = romeo.first;
+ se3::GeometryModel romeo_model_geom = romeo.second;
+ Data romeo_data(romeo_model);
+ GeometryData romeo_data_geom(romeo_data, romeo_model_geom);
+
+
+ std::vector<VectorXd> qs_romeo (NBT);
+ std::vector<VectorXd> qdots_romeo (NBT);
+ std::vector<VectorXd> qddots_romeo (NBT);
+ for(size_t i=0;i<NBT;++i)
+ {
+ qs_romeo[i] = Eigen::VectorXd::Random(romeo_model.nq);
+ qs_romeo[i].segment<4>(3) /= qs_romeo[i].segment<4>(3).norm();
+ qdots_romeo[i] = Eigen::VectorXd::Random(romeo_model.nv);
+ qddots_romeo[i] = Eigen::VectorXd::Random(romeo_model.nv);
+ }
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ geometry(romeo_model,romeo_data,qs_romeo[_smooth]);
+ }
+ double geom_time = timer.toc(StackTicToc::US)/NBT;
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ updateCollisionGeometry(romeo_model,romeo_data,romeo_model_geom,romeo_data_geom,qs_romeo[_smooth], true);
+ }
+ double update_col_time = timer.toc(StackTicToc::US)/NBT - geom_time;
+ std::cout << "Update Collision Geometry < false > = \t" << update_col_time << " " << StackTicToc::unitName(StackTicToc::US) << std::endl;
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ updateCollisionGeometry(romeo_model,romeo_data,romeo_model_geom,romeo_data_geom,qs_romeo[_smooth], true);
+ for (std::vector<se3::GeometryData::CollisionPair_t>::iterator it = romeo_data_geom.collision_pairs.begin(); it != romeo_data_geom.collision_pairs.end(); ++it)
+ {
+ romeo_data_geom.collide(it->first, it->second);
+ }
+ }
+ double collideTime = timer.toc(StackTicToc::US)/NBT - (update_col_time + geom_time);
+ std::cout << "Collision test between two geometry objects (mean time) = \t" << collideTime / romeo_data_geom.nCollisionPairs
+ << StackTicToc::unitName(StackTicToc::US) << std::endl;
+
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ computeCollisions(romeo_model,romeo_data,romeo_model_geom,romeo_data_geom,qs_romeo[_smooth], true);
+ }
+ double is_colliding_time = timer.toc(StackTicToc::US)/NBT - (update_col_time + geom_time);
+ std::cout << "Collision Test : robot in collision? = \t" << is_colliding_time
+ << StackTicToc::unitName(StackTicToc::US) << std::endl;
+
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ computeDistances(romeo_model,romeo_data,romeo_model_geom,romeo_data_geom,qs_romeo[_smooth]);
+ }
+ double computeDistancesTime = timer.toc(StackTicToc::US)/NBT - (update_col_time + geom_time);
+ std::cout << "Compute distance between two geometry objects (mean time) = \t" << computeDistancesTime / romeo_data_geom.nCollisionPairs
+ << " " << StackTicToc::unitName(StackTicToc::US) << " " << romeo_data_geom.nCollisionPairs << " col pairs" << std::endl;
+
+
+
+#ifdef WITH_HPP_MODEL_URDF
+
+
+
+ std::vector<VectorXd> qs_romeo_pino (NBT);
+ std::vector<VectorXd> qdots_romeo_pino (NBT);
+ std::vector<VectorXd> qddots_romeo_pino (NBT);
+ for(size_t i=0;i<NBT;++i)
+ {
+ qs_romeo_pino[i] = Eigen::VectorXd::Random(romeo_model.nq);
+ qs_romeo_pino[i].segment<4>(3) /= qs_romeo_pino[i].segment<4>(3).norm();
+ qdots_romeo_pino[i] = Eigen::VectorXd::Random(romeo_model.nv);
+ qddots_romeo_pino[i] = Eigen::VectorXd::Random(romeo_model.nv);
+ }
+ std::vector<VectorXd> qs_romeo_hpp (qs_romeo_pino);
+ std::vector<VectorXd> qdots_romeo_hpp (qdots_romeo_pino);
+ std::vector<VectorXd> qddots_romeo_hpp (qddots_romeo_pino);
+
+ for (size_t i = 0; i < NBT; ++i)
+ {
+ Vector4d quaternion;
+ quaternion << qs_romeo_pino[i][6], qs_romeo_pino[i][3], qs_romeo_pino[i][4], qs_romeo_pino[i][5];
+ qs_romeo_hpp[i].segment<4>(3) = quaternion ;
+ }
+
+
+
+
+// /// ************* HPP ************* ///
+// /// ********************************* ///
+
+
+hpp::model::HumanoidRobotPtr_t humanoidRobot =
+ hpp::model::HumanoidRobot::create ("romeo");
+ hpp::model::urdf::loadHumanoidModel(humanoidRobot, "freeflyer",
+ "romeo_pinocchio", "romeo",
+ "", "");
+
+
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ updateCollisionGeometry(romeo_model,romeo_data,romeo_model_geom,romeo_data_geom,qs_romeo_pino[_smooth], true);
+ }
+ double compute_forward_kinematics_time = timer.toc(StackTicToc::US)/NBT;
+ std::cout << "Update Collision Geometry < true > (K) = \t" << compute_forward_kinematics_time << " " << StackTicToc::unitName(StackTicToc::US) << std::endl;
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ humanoidRobot->currentConfiguration (qs_romeo_hpp[_smooth]);
+ humanoidRobot->computeForwardKinematics ();
+ }
+ double compute_forward_kinematics_time_hpp = timer.toc(StackTicToc::US)/NBT;
+ std::cout << "HPP - Compute Forward Kinematics (K) = \t" << compute_forward_kinematics_time_hpp
+ << StackTicToc::unitName(StackTicToc::US) << std::endl;
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ computeCollisions(romeo_data_geom, true);
+ }
+ double is_romeo_colliding_time_pino = timer.toc(StackTicToc::US)/NBT;
+ std::cout << "Pinocchio - Collision Test : robot in collision? (G) = \t" << is_romeo_colliding_time_pino
+ << StackTicToc::unitName(StackTicToc::US) << std::endl;
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ humanoidRobot->collisionTest();
+ }
+ double is_romeo_colliding_time_hpp = timer.toc(StackTicToc::US)/NBT;
+ std::cout << "HPP - Collision Test : robot in collision? (G)= \t" << is_romeo_colliding_time_hpp
+ << StackTicToc::unitName(StackTicToc::US) << std::endl;
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ computeCollisions(romeo_model,romeo_data,romeo_model_geom,romeo_data_geom,qs_romeo_pino[_smooth], true);
+ }
+ is_romeo_colliding_time_pino = timer.toc(StackTicToc::US)/NBT;
+ std::cout << "Pinocchio - Collision Test : update + robot in collision? (K+G)= \t" << is_romeo_colliding_time_pino
+ << StackTicToc::unitName(StackTicToc::US) << std::endl;
+
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ humanoidRobot->currentConfiguration (qs_romeo_hpp[_smooth]);
+ humanoidRobot->computeForwardKinematics ();
+ humanoidRobot->collisionTest();
+ }
+ is_romeo_colliding_time_hpp = timer.toc(StackTicToc::US)/NBT;
+ std::cout << "HPP - Collision Test : update + robot in collision? (K+G) = \t" << is_romeo_colliding_time_hpp
+ << StackTicToc::unitName(StackTicToc::US) << std::endl;
+
+
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ computeDistances(romeo_data_geom);
+ }
+ computeDistancesTime = timer.toc(StackTicToc::US)/NBT ;
+ std::cout << "Pinocchio - Compute distances (D) " << romeo_data_geom.nCollisionPairs << " col pairs\t" << computeDistancesTime
+ << " " << StackTicToc::unitName(StackTicToc::US) << std::endl;
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ humanoidRobot->computeDistances ();
+ }
+ double hpp_compute_distances = timer.toc(StackTicToc::US)/NBT ;
+ std::cout << "HPP - Compute distances (D) " << humanoidRobot->distanceResults().size() << " col pairs\t" << hpp_compute_distances
+ << " " << StackTicToc::unitName(StackTicToc::US) << std::endl;
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ computeDistances(romeo_model, romeo_data, romeo_model_geom, romeo_data_geom, qs_romeo_pino[_smooth]);
+ }
+ computeDistancesTime = timer.toc(StackTicToc::US)/NBT ;
+ std::cout << "Pinocchio - Update + Compute distances (K+D) " << romeo_data_geom.nCollisionPairs << " col pairs\t" << computeDistancesTime
+ << " " << StackTicToc::unitName(StackTicToc::US) << std::endl;
+
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ humanoidRobot->currentConfiguration (qs_romeo_hpp[_smooth]);
+ humanoidRobot->computeForwardKinematics ();
+ humanoidRobot->computeDistances ();
+ }
+ hpp_compute_distances = timer.toc(StackTicToc::US)/NBT ;
+ std::cout << "HPP - Update + Compute distances (K+D) " << humanoidRobot->distanceResults().size() << " col pairs\t" << hpp_compute_distances
+ << " " << StackTicToc::unitName(StackTicToc::US) << std::endl;
+
+#endif
+
+ return 0;
+}
diff --git a/benchmark/timings.cpp b/benchmark/timings.cpp
index 4acf2a2b401486f7b763d0050e6554ced3a036f1..e6cf7557bf0302cdc56bf6396680ff05b9931391 100644
--- a/benchmark/timings.cpp
+++ b/benchmark/timings.cpp
@@ -1,3 +1,20 @@
+//
+// Copyright (c) 2015 - 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/joint.hpp"
@@ -14,6 +31,7 @@
#include "pinocchio/multibody/parser/urdf.hpp"
#include "pinocchio/multibody/parser/sample-models.hpp"
+
#include <iostream>
#include "pinocchio/tools/timer.hpp"
@@ -137,6 +155,7 @@ int main(int argc, const char ** argv)
}
std::cout << "Geometry = \t"; timer.toc(std::cout,NBT);
+
timer.tic();
SMOOTH(NBT)
{
diff --git a/models/romeo.urdf b/models/romeo.urdf
index 1760f0483d1117b149b31d3d66e2ff79e7a7a17c..88a3da6a70d2373165f6365fb443e57f543612bb 100644
--- a/models/romeo.urdf
+++ b/models/romeo.urdf
@@ -6,6 +6,12 @@
<!-- Aldebaran file comes from -->
<!-- https://github.com/ros-aldebaran/romeo_robot/blob/master/romeo_description/urdf/romeoV1_generated_urdf/romeo.urdf -->
<robot name="romeoH37V1" xmlns:xacro="http://www.ros.org/wiki/xacro">
+ <link name="base_link"/>
+ <joint name="waist" type="fixed">
+ <parent link="base_link"/>
+ <child link="body"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </joint>
<joint name="NeckYaw" type="revolute">
<parent link="torso"/>
<child link="NeckYawLink"/>
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 68bc30d4f7719fb51cb9d7303e69b85f90d5a282..3b12df4f8d666fe5812911da91ae49026c75d772 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -67,6 +67,18 @@ IF (UNIX)
SET_TARGET_PROPERTIES( ${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} eigen3)
+ IF(URDFDOM_FOUND)
+ PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} urdfdom)
+ IF(HPP_FCL_FOUND)
+ PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} assimp)
+ ENDIF(HPP_FCL_FOUND)
+ ENDIF(URDFDOM_FOUND)
+
+ IF(HPP_FCL_FOUND)
+ PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} hpp-fcl)
+ ADD_TARGET_CFLAGS (${PROJECT_NAME} "-DWITH_HPP_FCL")
+ ENDIF(HPP_FCL_FOUND)
+
IF(LUA5_1_FOUND)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} lua5.1)
ENDIF(LUA5_1_FOUND)
@@ -91,7 +103,14 @@ IF(EIGENPY_FOUND)
PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} eigenpy)
IF(URDFDOM_FOUND)
PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} urdfdom)
+ IF(HPP_FCL_FOUND)
+ PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} assimp)
+ ENDIF(HPP_FCL_FOUND)
ENDIF(URDFDOM_FOUND)
+ IF(HPP_FCL_FOUND)
+ PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} hpp-fcl)
+ ADD_TARGET_CFLAGS (${PYWRAP} "-DWITH_HPP_FCL")
+ ENDIF(HPP_FCL_FOUND)
IF(LUA5_1_FOUND)
PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} lua5.1)
ENDIF(LUA5_1_FOUND)
diff --git a/src/algorithm/collisions.hpp b/src/algorithm/collisions.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..fc4f9df8c6393e908d274c95acec37ca4ddd49b5
--- /dev/null
+++ b/src/algorithm/collisions.hpp
@@ -0,0 +1,151 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_collisions_hpp__
+#define __se3_collisions_hpp__
+
+#include "pinocchio/multibody/visitor.hpp"
+#include "pinocchio/multibody/model.hpp"
+
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/multibody/geometry.hpp"
+
+namespace se3
+{
+
+
+ inline void updateCollisionGeometry(const Model & model,
+ Data & data,
+ const GeometryModel& geom,
+ GeometryData& data_geom,
+ const Eigen::VectorXd & q,
+ const bool computeGeometry = false
+ );
+
+ inline bool computeCollisions(const Model & model,
+ Data & data,
+ const GeometryModel & model_geom,
+ GeometryData & data_geom,
+ const Eigen::VectorXd & q,
+ const bool stopAtFirstCollision = false
+ );
+
+ inline bool computeCollisions(GeometryData & data_geom,
+ const bool stopAtFirstCollision = false
+ );
+
+ inline void computeDistances(GeometryData & data_geom);
+
+ inline void computeDistances(const Model & model,
+ Data & data,
+ const GeometryModel & model_geom,
+ GeometryData & data_geom,
+ const Eigen::VectorXd & q
+ );
+
+} // namespace se3
+
+/* --- Details -------------------------------------------------------------------- */
+namespace se3
+{
+
+
+inline void updateCollisionGeometry(const Model & model,
+ Data & data,
+ const GeometryModel & model_geom,
+ GeometryData & data_geom,
+ const Eigen::VectorXd & q,
+ const bool computeGeometry
+ )
+{
+ using namespace se3;
+
+ if (computeGeometry) geometry(model, data, q);
+ for (GeometryData::Index i=0; i < (GeometryData::Index) data_geom.model_geom.ngeom; ++i)
+ {
+ const Model::Index & parent = model_geom.geom_parents[i];
+ data_geom.oMg[i] = (data.oMi[parent] * model_geom.geometryPlacement[i]);
+ data_geom.oMg_fcl[i] = toFclTransform3f(data_geom.oMg[i]);
+ }
+}
+
+inline bool computeCollisions(GeometryData & data_geom,
+ const bool stopAtFirstCollision
+ )
+{
+ using namespace se3;
+
+ bool isColliding = false;
+
+ for (std::size_t cpt = 0; cpt < data_geom.collision_pairs.size(); ++cpt)
+ {
+ data_geom.collisions[cpt] = data_geom.collide(data_geom.collision_pairs[cpt].first, data_geom.collision_pairs[cpt].second);
+ isColliding = data_geom.collisions[cpt];
+ if(isColliding && stopAtFirstCollision)
+ {
+ return true;
+ }
+ }
+ return isColliding;
+}
+
+// WARNING, if stopAtFirstcollision = true, then the collisions vector will not be fulfilled.
+inline bool computeCollisions(const Model & model,
+ Data & data,
+ const GeometryModel & model_geom,
+ GeometryData & data_geom,
+ const Eigen::VectorXd & q,
+ const bool stopAtFirstCollision
+ )
+{
+ using namespace se3;
+
+
+ updateCollisionGeometry (model, data, model_geom, data_geom, q, true);
+
+
+ return computeCollisions(data_geom, stopAtFirstCollision);
+
+}
+
+
+inline void computeDistances(GeometryData & data_geom)
+{
+ for (std::size_t cpt = 0; cpt < data_geom.collision_pairs.size(); ++cpt)
+ {
+ data_geom.distances[cpt] = DistanceResult(data_geom.computeDistance(data_geom.collision_pairs[cpt].first, data_geom.collision_pairs[cpt].second),
+ data_geom.collision_pairs[cpt].first,
+ data_geom.collision_pairs[cpt].second
+ );
+ }
+}
+
+inline void computeDistances(const Model & model,
+ Data & data,
+ const GeometryModel & model_geom,
+ GeometryData & data_geom,
+ const Eigen::VectorXd & q
+ )
+{
+ updateCollisionGeometry (model, data, model_geom, data_geom, q, true);
+
+ computeDistances(data_geom);
+}
+} // namespace se3
+
+#endif // ifndef __se3_collisions_hpp__
+
diff --git a/src/algorithm/kinematics.hpp b/src/algorithm/kinematics.hpp
index 77a59ceec3e6f89f497501805afa8c296b6db6fc..3181e00fb62966fa5896d2f89bec9288dac23163 100644
--- a/src/algorithm/kinematics.hpp
+++ b/src/algorithm/kinematics.hpp
@@ -20,13 +20,15 @@
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
-
+
namespace se3
{
inline void geometry(const Model & model,
Data & data,
const Eigen::VectorXd & q);
+
+
inline void kinematics(const Model & model,
Data & data,
const Eigen::VectorXd & q,
@@ -90,6 +92,7 @@ namespace se3
}
}
+
struct KinematicsStep : public fusion::JointVisitor<KinematicsStep>
{
typedef boost::fusion::vector< const se3::Model&,
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..886209e3166b65f4b9a241f77d43730d85dad456
--- /dev/null
+++ b/src/multibody/geometry.hpp
@@ -0,0 +1,201 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_geom_hpp__
+#define __se3_geom_hpp__
+
+
+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/spatial/force.hpp"
+#include "pinocchio/spatial/motion.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/joint/joint-variant.hpp"
+#include <iostream>
+
+#include <hpp/fcl/collision_object.h>
+#include <hpp/fcl/collision.h>
+#include <hpp/fcl/distance.h>
+#include <map>
+#include <list>
+#include <utility>
+
+
+namespace se3
+{
+ class IsSameCollisionPair
+ {
+ typedef Model::Index Index;
+ typedef std::pair < Index, Index > CollisionPair_t;
+ public:
+ IsSameCollisionPair( CollisionPair_t pair): _pair(pair) {}
+
+ bool operator()(CollisionPair_t pair) const
+ {
+ return (pair == _pair);
+ }
+ private:
+ CollisionPair_t _pair;
+ };
+
+ // Result of distance computation between two CollisionObject.
+ struct DistanceResult
+ {
+ typedef Model::Index Index;
+
+ DistanceResult(fcl::DistanceResult dist_fcl, Index o1, Index o2)
+ : fcl_distance_result(dist_fcl), object1(o1), object2(o2)
+ {}
+
+ // Get distance between objects
+ double distance () const { return fcl_distance_result.min_distance; }
+
+ // Get closest point on inner object in global frame,
+ Eigen::Vector3d closestPointInner () const { return toVector3d(fcl_distance_result.nearest_points [0]); }
+
+ // Get closest point on outer object in global frame,
+ Eigen::Vector3d closestPointOuter () const { return toVector3d(fcl_distance_result.nearest_points [1]); }
+
+ bool operator == (const DistanceResult & other) const
+ {
+ return (distance() == other.distance()
+ && closestPointInner() == other.closestPointInner()
+ && closestPointOuter() == other.closestPointOuter()
+ && object1 == other.object1
+ && object2 == other.object2);
+ }
+ fcl::DistanceResult fcl_distance_result;
+ std::size_t object1;
+ std::size_t object2;
+ }; // struct DistanceResult
+
+ class GeometryModel
+ {
+ public:
+ typedef Model::Index Index;
+
+ Index ngeom;
+ std::vector<fcl::CollisionObject> collision_objects;
+ std::vector<std::string> geom_names;
+ std::vector<Index> geom_parents; // Joint parent of body <i>, denoted <li> (li==parents[i])
+ std::vector<SE3> geometryPlacement; // Position of geometry object in parent joint's frame
+
+ std::map < Index, std::list<Index> > innerObjects; // Associate a list of CollisionObjects to a given joint Id
+ std::map < Index, std::list<Index> > outerObjects; // Associate a list of CollisionObjects to a given joint Id
+
+ GeometryModel()
+ : ngeom(0)
+ , collision_objects()
+ , geom_names(0)
+ , geom_parents(0)
+ , geometryPlacement(0)
+ , innerObjects()
+ , outerObjects()
+ {
+ }
+
+ ~GeometryModel() {};
+
+ Index addGeomObject( Index parent,const fcl::CollisionObject & co, const SE3 & placement, const std::string & geoName = "");
+ Index getGeomId( const std::string & name ) const;
+ bool existGeomName( const std::string & name ) const;
+ const std::string& getGeomName( Index index ) const;
+
+ void addInnerObject(Index joint, Index inner_object);
+ void addOutterObject(Index joint, Index outer_object);
+
+ friend std::ostream& operator<<(std::ostream& os, const GeometryModel& model_geom);
+
+ private:
+
+ };
+
+ class GeometryData
+ {
+ public:
+ typedef Model::Index Index;
+ typedef std::pair < Index, Index > CollisionPair_t;
+
+ Data& data_ref;
+ GeometryModel& model_geom;
+
+ std::vector<se3::SE3> oMg;
+ std::vector<fcl::Transform3f> oMg_fcl;
+
+ std::vector < CollisionPair_t > collision_pairs;
+ Index nCollisionPairs;
+
+ std::vector < DistanceResult > distances;
+ std::vector < bool > collisions;
+
+ GeometryData(Data& data, GeometryModel& model_geom)
+ : data_ref(data)
+ , model_geom(model_geom)
+ , oMg(model_geom.ngeom)
+ , oMg_fcl(model_geom.ngeom)
+ , collision_pairs()
+ , nCollisionPairs(0)
+ , distances()
+ , collisions()
+ {
+ initializeListOfCollisionPairs();
+ distances.resize(nCollisionPairs, DistanceResult( fcl::DistanceResult(), 0, 0) );
+ collisions.resize(nCollisionPairs, false );
+
+ }
+
+ ~GeometryData() {};
+
+ void addCollisionPair (Index co1, Index co2);
+ void addCollisionPair (const CollisionPair_t& pair);
+ void removeCollisionPair (Index co1, Index co2);
+ void removeCollisionPair (const CollisionPair_t& pair);
+ bool isCollisionPair (Index co1, Index co2) const ;
+ bool isCollisionPair (const CollisionPair_t& pair) const;
+ void fillAllPairsAsCollisions();
+ void desactivateCollisionPairs();
+ void initializeListOfCollisionPairs();
+
+ bool collide(Index co1, Index co2) const;
+
+ fcl::DistanceResult computeDistance(Index co1, Index co2) const;
+ void resetDistances();
+
+ std::vector < DistanceResult > distanceResults(); //TODO : to keep or not depending of public or not for distances
+
+ void displayCollisionPairs() const
+ {
+ for (std::vector<CollisionPair_t>::const_iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
+ {
+ std::cout << it-> first << "\t" << it->second << std::endl;
+ }
+ }
+ friend std::ostream& operator<<(std::ostream& os, const GeometryData& data_geom);
+ private:
+
+ };
+
+} // namespace se3
+
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+#include "pinocchio/multibody/geometry.hxx"
+
+#endif // ifndef __se3_geom_hpp__
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
new file mode 100644
index 0000000000000000000000000000000000000000..c509baa367a9bbc3505ed860476fb06e44d35e4e
--- /dev/null
+++ b/src/multibody/geometry.hxx
@@ -0,0 +1,223 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_geom_hxx__
+#define __se3_geom_hxx__
+
+
+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/spatial/force.hpp"
+#include "pinocchio/spatial/motion.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/joint/joint-variant.hpp"
+#include <iostream>
+
+#include <hpp/fcl/collision_object.h>
+#include <hpp/fcl/collision.h>
+#include <hpp/fcl/distance.h>
+#include <map>
+#include <list>
+#include <utility>
+
+
+namespace se3
+{
+
+ inline GeometryModel::Index GeometryModel::addGeomObject( Index parent,const fcl::CollisionObject & co, const SE3 & placement, const std::string & geoName )
+ {
+
+ Index idx = (Index) (ngeom ++);
+
+
+ collision_objects .push_back(co);
+ geom_parents .push_back(parent);
+ geometryPlacement .push_back(placement);
+ geom_names .push_back( (geoName!="")?geoName:random(8));
+
+ return idx;
+ }
+
+ inline GeometryModel::Index GeometryModel::getGeomId( const std::string & name ) const
+ {
+ std::vector<std::string>::iterator::difference_type
+ res = std::find(geom_names.begin(),geom_names.end(),name) - geom_names.begin();
+ assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
+ assert( (res>=0)&&(res<(long)collision_objects.size())&&"The joint name you asked do not exist" );
+ return Index(res);
+ }
+
+ inline bool GeometryModel::existGeomName( const std::string & name ) const
+ {
+ return (geom_names.end() != std::find(geom_names.begin(),geom_names.end(),name));
+ }
+
+ inline const std::string& GeometryModel::getGeomName( Index index ) const
+ {
+ assert( index < (Index)collision_objects.size() );
+ return geom_names[index];
+ }
+
+ inline void GeometryModel::addInnerObject(Index joint, Index inner_object)
+ {
+ if (std::find(innerObjects[joint].begin(), innerObjects[joint].end(),inner_object)==innerObjects[joint].end())
+ innerObjects[joint].push_back(inner_object);
+ else
+ std::cout << "inner object already added" << std::endl;
+ }
+
+ inline void GeometryModel::addOutterObject(Index joint, Index outer_object)
+ {
+ if (std::find(outerObjects[joint].begin(), outerObjects[joint].end(),outer_object)==outerObjects[joint].end())
+ outerObjects[joint].push_back(outer_object);
+ else
+ std::cout << "outer object already added" << std::endl;
+ }
+
+ inline std::ostream& operator<<(std::ostream& os, const GeometryModel& model_geom)
+ {
+ os << "Nb collision objects = " << model_geom.ngeom << std::endl;
+
+ for(GeometryModel::Index i=0;i<(GeometryModel::Index)(model_geom.ngeom);++i)
+ {
+ os << "Object n " << i << " : " << model_geom.geom_names[i] << ": attached to joint = " << model_geom.geom_parents[i]
+ << "\nwith offset \n" << model_geom.geometryPlacement[i] <<std::endl;
+ }
+
+ return os;
+ }
+
+ inline std::ostream& operator<<(std::ostream& os, const GeometryData& data_geom)
+ {
+
+ for(GeometryData::Index i=0;i<(GeometryData::Index)(data_geom.model_geom.ngeom);++i)
+ {
+ os << "collision object oMi " << data_geom.oMg[i] << std::endl;
+ }
+
+ return os;
+ }
+
+ inline void GeometryData::addCollisionPair (Index co1, Index co2)
+ {
+ assert ( co1 < co2);
+ assert ( co2 < model_geom.ngeom);
+ CollisionPair_t pair(co1, co2);
+
+ addCollisionPair(pair);
+ }
+
+ inline void GeometryData::addCollisionPair (const CollisionPair_t& pair)
+ {
+ assert(pair.first < pair.second);
+ assert(pair.second < model_geom.ngeom);
+ collision_pairs.push_back(pair);
+ nCollisionPairs++;
+ }
+
+ inline void GeometryData::removeCollisionPair (Index co1, Index co2)
+ {
+ assert(co1 < co2);
+ assert(co2 < model_geom.ngeom);
+ assert(isCollisionPair(co1,co2));
+
+ removeCollisionPair (CollisionPair_t(co1,co2));
+ }
+
+ inline void GeometryData::removeCollisionPair (const CollisionPair_t& pair)
+ {
+ assert(pair.first < pair.second);
+ assert(pair.second < model_geom.ngeom);
+ assert(isCollisionPair(pair));
+
+ collision_pairs.erase(std::remove(collision_pairs.begin(), collision_pairs.end(), pair), collision_pairs.end());
+ nCollisionPairs--;
+ }
+
+ inline bool GeometryData::isCollisionPair (Index co1, Index co2) const
+ {
+ return isCollisionPair(CollisionPair_t(co1,co2));
+ }
+
+ inline bool GeometryData::isCollisionPair (const CollisionPair_t& pair) const
+ {
+ return (std::find_if ( collision_pairs.begin(), collision_pairs.end(),
+ IsSameCollisionPair(pair)) != collision_pairs.end());
+
+ }
+
+ inline void GeometryData::fillAllPairsAsCollisions()
+ {
+ for (Index i = 0; i < model_geom.ngeom; ++i)
+ {
+ for (Index j = i+1; j < model_geom.ngeom; ++j)
+ {
+ addCollisionPair(i,j);
+ }
+ }
+ }
+
+ // TODO : give a srdf file as argument, read it, and remove corresponding
+ // pairs from list collision_pairs
+ inline void GeometryData::desactivateCollisionPairs()
+ {
+
+ }
+
+ inline void GeometryData::initializeListOfCollisionPairs()
+ {
+ fillAllPairsAsCollisions();
+ desactivateCollisionPairs();
+ assert(nCollisionPairs == collision_pairs.size());
+ }
+
+ inline bool GeometryData::collide(Index co1, Index co2) const
+ {
+ fcl::CollisionRequest collisionRequest (1, false, false, 1, false, true, fcl::GST_INDEP);
+ fcl::CollisionResult collisionResult;
+
+ if (fcl::collide (model_geom.collision_objects[co1].collisionGeometry().get(), oMg_fcl[co1],
+ model_geom.collision_objects[co2].collisionGeometry().get(), oMg_fcl[co2],
+ collisionRequest, collisionResult) != 0)
+ {
+ return true;
+ }
+ return false;
+ }
+
+
+ inline fcl::DistanceResult GeometryData::computeDistance(Index co1, Index co2) const
+ {
+ fcl::DistanceRequest distanceRequest (true, 0, 0, fcl::GST_INDEP);
+ fcl::DistanceResult result;
+ fcl::distance ( model_geom.collision_objects[co1].collisionGeometry().get(), oMg_fcl[co1],
+ model_geom.collision_objects[co2].collisionGeometry().get(), oMg_fcl[co2],
+ distanceRequest, result);
+ return result;
+ }
+
+ inline void GeometryData::resetDistances()
+ {
+ std::fill(distances.begin(), distances.end(), DistanceResult( fcl::DistanceResult(), 0, 0) );
+ }
+
+
+} // namespace se3
+
+#endif // ifndef __se3_geom_hxx__
diff --git a/src/multibody/parser/from-collada-to-fcl.hpp b/src/multibody/parser/from-collada-to-fcl.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..75938acb549fd34b43da4a859c2e8c0f9d4697d8
--- /dev/null
+++ b/src/multibody/parser/from-collada-to-fcl.hpp
@@ -0,0 +1,207 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio and is mainly inspired
+// by software hpp-model-urdf
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_collada_to_fcl_hpp__
+#define __se3_collada_to_fcl_hpp__
+
+#include <limits>
+
+#include <boost/filesystem/fstream.hpp>
+#include <boost/foreach.hpp>
+#include <boost/format.hpp>
+
+#include <assimp/DefaultLogger.h>
+
+#include <assimp/assimp.hpp>
+#include <assimp/aiScene.h>
+#include <assimp/aiPostProcess.h>
+#include <assimp/IOStream.h>
+#include <assimp/IOSystem.h>
+
+
+#include <hpp/fcl/BV/OBBRSS.h>
+#include <hpp/fcl/BVH/BVH_model.h>
+
+typedef fcl::BVHModel< fcl::OBBRSS > PolyhedronType;
+typedef boost::shared_ptr <PolyhedronType> PolyhedronPtrType;
+
+struct TriangleAndVertices
+{
+ void clear()
+ {
+ vertices_.clear ();
+ triangles_.clear ();
+ }
+ std::vector <fcl::Vec3f> vertices_;
+ std::vector <fcl::Triangle> triangles_;
+};
+
+
+/**
+ * @brief Recursive procedure for building a mesh
+ *
+ * @param[in] scale Scale to apply when reading the ressource
+ * @param[in] scene Pointer to the assimp scene
+ * @param[in] node Current node of the scene
+ * @param subMeshIndexes Submesh triangles indexes interval
+ * @param[in] mesh The mesh that must be built
+ * @param tv Triangles and Vertices of the mesh submodels
+ */
+inline void buildMesh (const ::urdf::Vector3& scale, const aiScene* scene, const aiNode* node,
+ std::vector<unsigned>& subMeshIndexes, const PolyhedronPtrType& mesh,
+ TriangleAndVertices& tv)
+{
+ if (!node) return;
+
+ aiMatrix4x4 transform = node->mTransformation;
+ aiNode *pnode = node->mParent;
+ while (pnode)
+ {
+ // Don't convert to y-up orientation, which is what the root node in
+ // Assimp does
+ if (pnode->mParent != NULL)
+ {
+ transform = pnode->mTransformation * transform;
+ }
+ pnode = pnode->mParent;
+ }
+
+ for (uint32_t i = 0; i < node->mNumMeshes; i++)
+ {
+ aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
+
+ unsigned oldNbPoints = mesh->num_vertices;
+ unsigned oldNbTriangles = mesh->num_tris;
+
+ // Add the vertices
+ for (uint32_t j = 0; j < input_mesh->mNumVertices; j++)
+ {
+ aiVector3D p = input_mesh->mVertices[j];
+ p *= transform;
+ tv.vertices_.push_back (fcl::Vec3f (p.x * scale.x,
+ p.y * scale.y,
+ p.z * scale.z));
+ }
+
+ // add the indices
+ for (uint32_t j = 0; j < input_mesh->mNumFaces; j++)
+ {
+ aiFace& face = input_mesh->mFaces[j];
+ // FIXME: can add only triangular faces.
+ tv.triangles_.push_back (fcl::Triangle( oldNbPoints + face.mIndices[0],
+ oldNbPoints + face.mIndices[1],
+ oldNbPoints + face.mIndices[2]));
+ }
+
+ // Save submesh triangles indexes interval.
+ if (subMeshIndexes.size () == 0)
+ {
+ subMeshIndexes.push_back (0);
+ }
+
+ subMeshIndexes.push_back (oldNbTriangles + input_mesh->mNumFaces);
+ }
+
+ for (uint32_t i=0; i < node->mNumChildren; ++i)
+ {
+ buildMesh(scale, scene, node->mChildren[i], subMeshIndexes, mesh, tv);
+ }
+}
+
+
+
+/**
+ * @brief Convert an assimp scene to a mesh
+ *
+ * @param[in] name File (ressource) transformed into an assimp scene in loa
+ * @param[in] scale Scale to apply when reading the ressource
+ * @param[in] scene Pointer to the assimp scene
+ * @param[in] mesh The mesh that must be built
+ */
+inline void meshFromAssimpScene (const std::string& name, const ::urdf::Vector3& scale,
+ const aiScene* scene,const PolyhedronPtrType& mesh)
+ {
+ TriangleAndVertices tv;
+
+ if (!scene->HasMeshes())
+ {
+ throw std::runtime_error (std::string ("No meshes found in file ")+
+ name);
+ }
+
+ std::vector<unsigned> subMeshIndexes;
+ int res = mesh->beginModel ();
+
+ if (res != fcl::BVH_OK)
+ {
+ std::ostringstream error;
+ error << "fcl BVHReturnCode = " << res;
+ throw std::runtime_error (error.str ());
+ }
+
+ tv.clear();
+
+ buildMesh (scale, scene, scene->mRootNode, subMeshIndexes, mesh, tv);
+ mesh->addSubModel (tv.vertices_, tv.triangles_);
+
+ mesh->endModel ();
+ }
+
+
+/**
+ * @brief Read a mesh file and convert it to a polyhedral mesh
+ *
+ * @param[in] resource_path Path to the ressource mesh file to be read
+ * @param[in] scale Scale to apply when reading the ressource
+ * @param[in] polyhedron The resulted polyhedron
+ */
+inline void loadPolyhedronFromResource ( const std::string& resource_path, const ::urdf::Vector3& scale,
+ const PolyhedronPtrType& polyhedron)
+{
+ Assimp::Importer importer;
+ const aiScene* scene = importer.ReadFile(resource_path.c_str(), aiProcess_SortByPType| aiProcess_GenNormals|
+ aiProcess_Triangulate|aiProcess_GenUVCoords|
+ aiProcess_FlipUVs);
+ if (!scene)
+ {
+ throw std::runtime_error (std::string ("Could not load resource ") + resource_path + std::string ("\n") +
+ importer.GetErrorString ());
+ }
+
+ meshFromAssimpScene (resource_path, scale, scene, polyhedron);
+}
+
+
+/**
+ * @brief Transform a cURL readable path (package://..) to an absolute path for urdf collision path
+ *
+ * @param urdf_mesh_path The path given in the urdf file (package://..)
+ * @param[in] meshRootDir Root path to the directory where meshes are located
+ *
+ * @return The absolute path to the mesh file
+ */
+inline std::string fromURDFMeshPathToAbsolutePath(std::string & urdf_mesh_path, std::string meshRootDir)
+{
+
+ std::string absolutePath = std::string(meshRootDir +
+ urdf_mesh_path.substr(10, urdf_mesh_path.size()));
+
+ return absolutePath;
+}
+
+#endif // __se3_collada_to_fcl_hpp__
diff --git a/src/multibody/parser/sample-models.cpp b/src/multibody/parser/sample-models.cpp
index fefdc1ad643486eb5650d31ee08b838a9560debc..9cdc2d4f960b90c7fcf8817eec0e7b63bea6b2ba 100644
--- a/src/multibody/parser/sample-models.cpp
+++ b/src/multibody/parser/sample-models.cpp
@@ -18,6 +18,10 @@
#include "pinocchio/multibody/parser/sample-models.hpp"
+#ifdef WITH_HPP_FCL
+#include <hpp/fcl/shape/geometric_shapes.h>
+#endif
+
namespace se3
{
namespace buildModels
@@ -193,5 +197,24 @@ namespace se3
"larm6_joint", "larm6_body");
}
+ #ifdef WITH_HPP_FCL
+ void collisionModel( Model& model, GeometryModel& model_geom)
+ {
+ model.addBody(model.getBodyId("universe"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
+ "planar1_joint", "planar1_body");
+ model.addBody(model.getBodyId("universe"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
+ "planar2_joint", "planar2_body");
+
+ boost::shared_ptr<fcl::Box> Sample(new fcl::Box(1));
+ fcl::CollisionObject box1(Sample, fcl::Transform3f());
+ model_geom.addGeomObject(model.getJointId("planar1_joint"),box1, SE3::Identity(), "ff1_collision_object");
+
+ boost::shared_ptr<fcl::Box> Sample2(new fcl::Box(1));
+ fcl::CollisionObject box2(Sample, fcl::Transform3f());
+ model_geom.addGeomObject(model.getJointId("planar2_joint"),box2, SE3::Identity(), "ff2_collision_object");
+
+ }
+ #endif
+
} // namespace buildModels
} // namespace se3
diff --git a/src/multibody/parser/sample-models.hpp b/src/multibody/parser/sample-models.hpp
index e618bc16f482fbd78227afa46efde4f5fd6d0cfa..405379545033479a684ca5c12a2c03ea5cb4f954 100644
--- a/src/multibody/parser/sample-models.hpp
+++ b/src/multibody/parser/sample-models.hpp
@@ -21,6 +21,10 @@
#include "pinocchio/multibody/model.hpp"
+#ifdef WITH_HPP_FCL
+#include "pinocchio/multibody/geometry.hpp"
+#endif
+
namespace se3
{
namespace buildModels
@@ -30,6 +34,10 @@ namespace se3
void humanoidSimple(Model& model, bool usingFF = true);
+ #ifdef WITH_HPP_FCL
+ void collisionModel( Model& model, GeometryModel& geom);
+ #endif
+
} // namespace buildModels
} // namespace se3
diff --git a/src/multibody/parser/urdf-with-geometry.hpp b/src/multibody/parser/urdf-with-geometry.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a5a26032f596298144a804c1ec383e6234c39b23
--- /dev/null
+++ b/src/multibody/parser/urdf-with-geometry.hpp
@@ -0,0 +1,107 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_urdf_geom_hpp__
+#define __se3_urdf_geom_hpp__
+
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+
+#include <iostream>
+#include <boost/foreach.hpp>
+#include "pinocchio/multibody/model.hpp"
+
+#include <hpp/fcl/collision_object.h>
+#include <hpp/fcl/collision.h>
+#include <hpp/fcl/shape/geometric_shapes.h>
+#include "pinocchio/multibody/parser/from-collada-to-fcl.hpp"
+
+#include <exception>
+
+namespace urdf
+{
+ typedef boost::shared_ptr<ModelInterface> ModelInterfacePtr;
+ typedef boost::shared_ptr<const Joint> JointConstPtr;
+ typedef boost::shared_ptr<const Link> LinkConstPtr;
+ typedef boost::shared_ptr<Link> LinkPtr;
+ typedef boost::shared_ptr<const Inertial> InertialConstPtr;
+}
+
+namespace se3
+{
+ namespace urdf
+ {
+
+
+ /**
+ * @brief Get a CollisionObject from an urdf link, reading the
+ * corresponding mesh
+ *
+ * @param[in] link The input urdf link
+ * @param[in] meshRootDir Root path to the directory where meshes are located
+ *
+ * @return The mesh converted as a fcl::CollisionObject
+ */
+ inline fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link, const std::string & meshRootDir);
+
+
+ /**
+ * @brief Recursive procedure for reading the URDF tree, looking for geometries
+ * This function fill the geometric model whith geometry objects retrieved from the URDF tree
+ *
+ * @param[in] link The current URDF link
+ * @param model The model to which is the Geometry Model associated
+ * @param model_geom The Geometry Model where the Collision Objects must be added
+ * @param[in] meshRootDir Root path to the directory where meshes are located
+ */
+ inline void parseTreeForGeom( ::urdf::LinkConstPtr link, Model & model,GeometryModel & model_geom, const std::string & meshRootDir, const bool rootJointAdded) throw (std::invalid_argument);
+
+
+
+ /**
+ * @brief Build The Model and GeometryModel from a URDF file with a particular joint as root of the model tree
+ *
+ * @param[in] filename The URDF complete file path
+ * @param[in] meshRootDir Root path to the directory where meshes are located
+ *
+ * @return The pair <se3::Model, se3::GeometryModel> the Model tree and its geometric model associated
+ */
+ template <typename D>
+ std::pair<Model, GeometryModel > buildModelAndGeom( const std::string & filename, const std::string & meshRootDir, const JointModelBase<D> & root_joint );
+
+
+ /**
+ * @brief Build The Model and GeometryModel from a URDF file with no root joint added to the model tree
+ *
+ * @param[in] filename The URDF complete file path
+ * @param[in] meshRootDir Root path to the directory where meshes are located
+ *
+ * @return The pair <se3::Model, se3::GeometryModel> the Model tree and its geometric model associated
+ */
+ inline std::pair<Model, GeometryModel > buildModelAndGeom( const std::string & filename, const std::string & meshRootDir);
+
+ } // namespace urdf
+} // namespace se3
+
+
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+#include "pinocchio/multibody/parser/urdf-with-geometry.hxx"
+
+#endif // ifndef __se3_urdf_geom_hpp__
+
diff --git a/src/multibody/parser/urdf-with-geometry.hxx b/src/multibody/parser/urdf-with-geometry.hxx
new file mode 100644
index 0000000000000000000000000000000000000000..a5b0703e9a7f3cd706a9b16659d16990fddd96f7
--- /dev/null
+++ b/src/multibody/parser/urdf-with-geometry.hxx
@@ -0,0 +1,147 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_urdf_geom_hxx__
+#define __se3_urdf_geom_hxx__
+
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+
+#include <iostream>
+#include <boost/foreach.hpp>
+#include "pinocchio/multibody/model.hpp"
+
+#include <hpp/fcl/collision_object.h>
+#include <hpp/fcl/collision.h>
+#include <hpp/fcl/shape/geometric_shapes.h>
+#include "pinocchio/multibody/parser/from-collada-to-fcl.hpp"
+
+#include <exception>
+
+
+namespace se3
+{
+ namespace urdf
+ {
+
+
+ inline fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link, const std::string & meshRootDir)
+ {
+ boost::shared_ptr < ::urdf::Collision> collision = link->collision;
+ boost::shared_ptr < fcl::CollisionGeometry > geometry;
+
+ // Handle the case where collision geometry is a mesh
+ if (collision->geometry->type == ::urdf::Geometry::MESH)
+ {
+ boost::shared_ptr < ::urdf::Mesh> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Mesh> (collision->geometry);
+ std::string collisionFilename = collisionGeometry->filename;
+
+ std::string full_path = fromURDFMeshPathToAbsolutePath(collisionFilename, meshRootDir);
+
+ ::urdf::Vector3 scale = collisionGeometry->scale;
+
+ // Create FCL mesh by parsing Collada file.
+ PolyhedronPtrType polyhedron (new PolyhedronType);
+
+ loadPolyhedronFromResource (full_path, scale, polyhedron);
+ geometry = polyhedron;
+ }
+
+ if (!geometry)
+ {
+ throw std::runtime_error(std::string("The polyhedron retrived is empty"));
+ }
+ fcl::CollisionObject collisionObject (geometry, fcl::Transform3f());
+
+ return collisionObject; // TO CHECK: what happens if geometry is empty ?
+ }
+
+
+ inline void parseTreeForGeom( ::urdf::LinkConstPtr link, Model & model,GeometryModel & model_geom, const std::string & meshRootDir, const bool rootJointAdded) throw (std::invalid_argument)
+ {
+
+ // start with first link that is not empty
+ if(link->inertial)
+ {
+ ::urdf::JointConstPtr joint = link->parent_joint;
+
+ if (joint == NULL && rootJointAdded )
+ {
+
+ if (link->collision)
+ {
+ fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, meshRootDir);
+ SE3 geomPlacement = convertFromUrdf(link->collision->origin);
+ std::string collision_object_name = link->name ;
+ model_geom.addGeomObject(model.getJointId("root_joint"), collision_object, geomPlacement, collision_object_name);
+ }
+ }
+
+ if(joint!=NULL)
+ {
+ assert(link->getParent()!=NULL);
+
+ if (link->collision)
+ {
+ fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, meshRootDir);
+ SE3 geomPlacement = convertFromUrdf(link->collision->origin);
+ std::string collision_object_name = link->name ;
+ model_geom.addGeomObject(model.getJointId(joint->name), collision_object, geomPlacement, collision_object_name);
+ }
+ }
+ else if (link->getParent() != NULL)
+ {
+ const std::string exception_message (link->name + " - joint information missing.");
+ throw std::invalid_argument(exception_message);
+ }
+
+ }
+
+ BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
+ {
+ parseTreeForGeom(child, model, model_geom, meshRootDir, rootJointAdded);
+ }
+ }
+
+
+
+ template <typename D>
+ std::pair<Model, GeometryModel > buildModelAndGeom( const std::string & filename, const std::string & meshRootDir, const JointModelBase<D> & root_joint )
+ {
+ Model model; GeometryModel model_geom;
+
+ ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
+ parseTree(urdfTree->getRoot(), model, SE3::Identity(), root_joint);
+ parseTreeForGeom(urdfTree->getRoot(), model, model_geom, meshRootDir, true);
+ return std::make_pair(model, model_geom);
+ }
+
+ inline std::pair<Model, GeometryModel > buildModelAndGeom( const std::string & filename, const std::string & meshRootDir)
+ {
+ Model model; GeometryModel model_geom;
+
+ ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
+ parseTree(urdfTree->getRoot(), model, SE3::Identity());
+ parseTreeForGeom(urdfTree->getRoot(), model, model_geom, meshRootDir, false);
+ return std::make_pair(model, model_geom);
+ }
+
+ } // namespace urdf
+} // namespace se3
+
+#endif // ifndef __se3_urdf_geom_hxx__
+
diff --git a/src/multibody/parser/urdf.hxx b/src/multibody/parser/urdf.hxx
index de5f9a2d4e01fcf9d0dcf19339d6d0345f0b2736..ebf258f2c8bb05057b6d439cae06f2b9383b90e1 100644
--- a/src/multibody/parser/urdf.hxx
+++ b/src/multibody/parser/urdf.hxx
@@ -389,23 +389,46 @@ namespace se3
}
- template <typename D>
+ template <typename D>
void parseTree (::urdf::LinkConstPtr link, Model & model, const SE3 & placementOffset, const JointModelBase<D> & root_joint, const bool verbose) throw (std::invalid_argument)
{
if (!link->inertial)
{
- const std::string exception_message (link->name + " - spatial inertia information missing.");
- throw std::invalid_argument(exception_message);
- }
-
- const Inertia & Y = convertFromUrdf(*link->inertial);
- const bool has_visual = (link->visual) ? true : false;
- model.addBody(0, root_joint, placementOffset, Y , "root_joint", link->name, has_visual);
+ // If the root link has only one child
+ if (link->child_links.size() == 1)
+ {
+ ::urdf::LinkPtr child_link = link->child_links[0];
+ const Inertia & Y = convertFromUrdf(*child_link->inertial);
+ const bool has_visual = (child_link->visual) ? true : false;
+ model.addBody(0, root_joint, placementOffset, Y, "root_joint", child_link->name, has_visual);
+
+ // Change the name of the parent joint
+ child_link->parent_joint->name = "root_joint";
+
+ BOOST_FOREACH(::urdf::LinkConstPtr child, child_link->child_links)
+ {
+ parseTree(child, model, SE3::Identity(), verbose);
+ }
+ }
+ else
+ {
+ const std::string exception_message (link->name + " - spatial inertial information missing with more than one child.");
+ throw std::invalid_argument(exception_message);
+ }
- BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
+ }
+ else
{
- parseTree(child, model, SE3::Identity(), verbose);
+ const Inertia & Y = convertFromUrdf(*link->inertial);
+ const bool has_visual = (link->visual) ? true : false;
+ model.addBody(0, root_joint, placementOffset, Y , "root_joint", link->name, has_visual);
+
+ BOOST_FOREACH(::urdf::LinkConstPtr child, link->child_links)
+ {
+ parseTree(child, model, SE3::Identity(), verbose);
+ }
}
+
}
diff --git a/src/python/algorithms.hpp b/src/python/algorithms.hpp
index c971a6cc43fa58675342afa62627d5788a2be3d3..3c62c05fee19a3e2b4cdcae9e1ce25d0dfdde435 100644
--- a/src/python/algorithms.hpp
+++ b/src/python/algorithms.hpp
@@ -34,6 +34,13 @@
#include "pinocchio/simulation/compute-all-terms.hpp"
+#ifdef WITH_HPP_FCL
+#include "pinocchio/multibody/geometry.hpp"
+#include "pinocchio/python/geometry-model.hpp"
+#include "pinocchio/python/geometry-data.hpp"
+#include "pinocchio/algorithm/collisions.hpp"
+#endif
+
namespace se3
{
namespace python
@@ -147,6 +154,39 @@ namespace se3
jointLimits(*model,*data,q);
}
+#ifdef WITH_HPP_FCL
+ static bool computeCollisions_proxy(GeometryDataHandler & data_geom,
+ const bool stopAtFirstCollision)
+ {
+ return computeCollisions(*data_geom, stopAtFirstCollision);
+ }
+
+ static bool computeGeometryAndCollisions_proxy(const ModelHandler & model,
+ DataHandler & data,
+ const GeometryModelHandler & model_geom,
+ GeometryDataHandler & data_geom,
+ const VectorXd_fx & q,
+ const bool & stopAtFirstCollision)
+ {
+ return computeCollisions(*model,*data,*model_geom, *data_geom, q, stopAtFirstCollision);
+ }
+
+ static void computeDistances_proxy(GeometryDataHandler & data_geom)
+ {
+ computeDistances(*data_geom);
+ }
+
+ static void computeGeometryAndDistances_proxy( const ModelHandler & model,
+ DataHandler & data,
+ const GeometryModelHandler & model_geom,
+ GeometryDataHandler & data_geom,
+ const Eigen::VectorXd & q
+ )
+ {
+ computeDistances(*model, *data, *model_geom, *data_geom, q);
+ }
+
+#endif
/* --- Expose --------------------------------------------------------- */
@@ -236,8 +276,32 @@ namespace se3
"Configuration q (size Model::nq)"),
"Compute the maximum limits of all the joints of the model "
"and put the results in data.");
- }
+#ifdef WITH_HPP_FCL
+ bp::def("computeCollisions",computeCollisions_proxy,
+ bp::args("GeometryData","bool"),
+ "Determine if collisions pairs are effectively in collision."
+ );
+
+ bp::def("computeGeometryAndCollisions",computeGeometryAndCollisions_proxy,
+ bp::args("Model","Data","GeometryModel","GeometryData","Configuration q (size Model::nq)", "bool"),
+ "Update the geometry for a given configuration and"
+ "determine if collision pars are effectively in collision"
+ );
+
+ bp::def("computeDistances",computeDistances_proxy,
+ bp::args("GeometryData"),
+ "Compute the distance between each collision pairs."
+ );
+
+ bp::def("computeGeometryAndDistances",computeGeometryAndDistances_proxy,
+ bp::args("Model","Data","GeometryModel","GeometryData","Configuration q (size Model::nq)"),
+ "Update the geometry for a given configuration and"
+ "compute the distance between each collision pairs"
+ );
+
+ }
+#endif
};
}} // namespace se3::python
diff --git a/src/python/geometry-data.hpp b/src/python/geometry-data.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5cf4996195edd056d444afefe5c7790fe8304eb3
--- /dev/null
+++ b/src/python/geometry-data.hpp
@@ -0,0 +1,139 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_python_geometry_data_hpp__
+#define __se3_python_geometry_data_hpp__
+
+#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
+#include <eigenpy/exception.hpp>
+#include <eigenpy/eigenpy.hpp>
+
+// #include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/python/se3.hpp"
+#include "pinocchio/python/eigen_container.hpp"
+#include "pinocchio/python/handler.hpp"
+
+#include "pinocchio/multibody/geometry.hpp"
+
+namespace se3
+{
+ namespace python
+ {
+ namespace bp = boost::python;
+
+ typedef Handler<GeometryData> GeometryDataHandler;
+
+ struct GeometryDataPythonVisitor
+ : public boost::python::def_visitor< GeometryDataPythonVisitor >
+ {
+ public:
+ typedef GeometryData::Index Index;
+ typedef GeometryData::CollisionPair_t CollisionPair_t;
+ typedef se3::DistanceResult DistanceResult;
+ typedef eigenpy::UnalignedEquivalent<SE3>::type SE3_fx;
+
+ public:
+
+ /* --- Convert From C++ to Python ------------------------------------- */
+ // static PyObject* convert(Model const& modelConstRef)
+ // {
+ // Model * ptr = const_cast<Model*>(&modelConstRef);
+ // return boost::python::incref(boost::python::object(ModelHandler(ptr)).ptr());
+ // }
+ static PyObject* convert(GeometryDataHandler::SmartPtr_t const& ptr)
+ {
+ return boost::python::incref(boost::python::object(GeometryDataHandler(ptr)).ptr());
+ }
+
+ /* --- Exposing C++ API to python through the handler ----------------- */
+ template<class PyClass>
+ void visit(PyClass& cl) const
+ {
+ cl
+ .add_property("nCollisionPairs", &GeometryDataPythonVisitor::nCollisionPairs)
+
+ .add_property("oMg",
+ bp::make_function(&GeometryDataPythonVisitor::oMg,
+ bp::return_internal_reference<>()) )
+ .add_property("collision_pairs",
+ bp::make_function(&GeometryDataPythonVisitor::collision_pairs,
+ bp::return_internal_reference<>()) )
+ .add_property("distances",
+ bp::make_function(&GeometryDataPythonVisitor::distances,
+ bp::return_internal_reference<>()) )
+ .add_property("collisions",
+ bp::make_function(&GeometryDataPythonVisitor::collisions,
+ bp::return_internal_reference<>()) )
+
+ .def("addCollisionPair",&GeometryDataPythonVisitor::isCollisionPair)
+ .def("removeCollisionPair",&GeometryDataPythonVisitor::isCollisionPair)
+ .def("isCollisionPair",&GeometryDataPythonVisitor::isCollisionPair)
+ .def("collide",&GeometryDataPythonVisitor::collide)
+
+ .def("__str__",&GeometryDataPythonVisitor::toString)
+
+
+ ;
+ }
+
+ static GeometryModel::Index nCollisionPairs( GeometryDataHandler & m ) { return m->nCollisionPairs; }
+
+ static std::vector<SE3> & oMg( GeometryDataHandler & m ) { return m->oMg; }
+ static std::vector<CollisionPair_t> & collision_pairs( GeometryDataHandler & m ) { return m->collision_pairs; }
+ static std::vector<DistanceResult> & distances( GeometryDataHandler & m ) { return m->distances; }
+ static std::vector<bool> & collisions( GeometryDataHandler & m ) { return m->collisions; }
+
+ static void addCollisionPair (GeometryDataHandler & m, Index co1, Index co2) { m -> addCollisionPair(co1, co2);}
+ static void removeCollisionPair (GeometryDataHandler & m, Index co1, Index co2) { m -> removeCollisionPair(co1, co2);}
+ static bool isCollisionPair (const GeometryDataHandler & m, Index co1, Index co2) { return m -> isCollisionPair(co1, co2);}
+
+ static bool collide(const GeometryDataHandler & m, Index co1, Index co2) { return m -> collide(co1, co2); };
+
+
+
+ static std::string toString(const GeometryDataHandler& m)
+ { std::ostringstream s; s << *m; return s.str(); }
+
+ /* --- Expose --------------------------------------------------------- */
+ static void expose()
+ {
+
+ bp::class_<GeometryDataHandler>("GeometryData",
+ "Geometry data ",
+ bp::no_init)
+ .def(GeometryDataPythonVisitor());
+
+ bp::class_< std::vector<CollisionPair_t> >("StdVec_CollisionPair_t")
+ .def(bp::vector_indexing_suite< std::vector<CollisionPair_t> >());
+ bp::class_< std::vector<DistanceResult> >("StdVec_DistanceResult")
+ .def(bp::vector_indexing_suite< std::vector<DistanceResult> >());
+
+ /* Not sure if it is a good idea to enable automatic
+ * conversion. Prevent it for now */
+ //bp::to_python_converter< Model,GeometryDataPythonVisitor >();
+ bp::to_python_converter< GeometryDataHandler::SmartPtr_t,GeometryDataPythonVisitor >();
+ }
+
+
+ };
+
+
+
+ }} // namespace se3::python
+
+#endif // ifndef __se3_python_geometry_data_hpp__
+
diff --git a/src/python/geometry-model.hpp b/src/python/geometry-model.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..6d7396ecc5e05960c394afd4d6df1bb94794a122
--- /dev/null
+++ b/src/python/geometry-model.hpp
@@ -0,0 +1,129 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_python_geometry_model_hpp__
+#define __se3_python_geometry_model_hpp__
+
+#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
+#include <eigenpy/exception.hpp>
+#include <eigenpy/eigenpy.hpp>
+
+// #include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/python/se3.hpp"
+#include "pinocchio/python/eigen_container.hpp"
+#include "pinocchio/python/handler.hpp"
+
+#include "pinocchio/multibody/geometry.hpp"
+
+namespace se3
+{
+ namespace python
+ {
+ namespace bp = boost::python;
+
+ typedef Handler<GeometryModel> GeometryModelHandler;
+
+ struct GeometryModelPythonVisitor
+ : public boost::python::def_visitor< GeometryModelPythonVisitor >
+ {
+ public:
+ typedef GeometryModel::Index Index;
+ typedef eigenpy::UnalignedEquivalent<SE3>::type SE3_fx;
+
+ public:
+
+ /* --- Convert From C++ to Python ------------------------------------- */
+ // static PyObject* convert(Model const& modelConstRef)
+ // {
+ // Model * ptr = const_cast<Model*>(&modelConstRef);
+ // return boost::python::incref(boost::python::object(ModelHandler(ptr)).ptr());
+ // }
+ static PyObject* convert(GeometryModelHandler::SmartPtr_t const& ptr)
+ {
+ return boost::python::incref(boost::python::object(GeometryModelHandler(ptr)).ptr());
+ }
+
+ /* --- Exposing C++ API to python through the handler ----------------- */
+ template<class PyClass>
+ void visit(PyClass& cl) const
+ {
+ cl
+ .add_property("ngeom", &GeometryModelPythonVisitor::ngeom)
+
+ .def("getGeomId",&GeometryModelPythonVisitor::getGeomId)
+
+ .add_property("geometryPlacement",
+ bp::make_function(&GeometryModelPythonVisitor::geometryPlacement,
+ bp::return_internal_reference<>()) )
+ .add_property("geom_parents",
+ bp::make_function(&GeometryModelPythonVisitor::geom_parents,
+ bp::return_internal_reference<>()) )
+ .add_property("geom_names",
+ bp::make_function(&GeometryModelPythonVisitor::geom_names,
+ bp::return_internal_reference<>()) )
+
+ .def("__str__",&GeometryModelPythonVisitor::toString)
+
+ .def("BuildEmptyGeometryModel",&GeometryModelPythonVisitor::maker_empty)
+ .staticmethod("BuildEmptyGeometryModel")
+ ;
+ }
+
+ static GeometryModel::Index ngeom( GeometryModelHandler & m ) { return m->ngeom; }
+
+ static Model::Index getGeomId( const GeometryModelHandler & gmodelPtr, const std::string & name )
+ { return gmodelPtr->getGeomId(name); }
+
+ static std::vector<SE3> & geometryPlacement( GeometryModelHandler & m ) { return m->geometryPlacement; }
+ static std::vector<Model::Index> & geom_parents( GeometryModelHandler & m ) { return m->geom_parents; }
+ static std::vector<std::string> & geom_names ( GeometryModelHandler & m ) { return m->geom_names; }
+
+
+
+ static GeometryModelHandler maker_empty()
+ {
+ return GeometryModelHandler( new GeometryModel(),true );
+ }
+
+
+ static std::string toString(const GeometryModelHandler& m)
+ { std::ostringstream s; s << *m; return s.str(); }
+
+ /* --- Expose --------------------------------------------------------- */
+ static void expose()
+ {
+
+ bp::class_<GeometryModelHandler>("GeometryModel",
+ "Geometry model (const)",
+ bp::no_init)
+ .def(GeometryModelPythonVisitor());
+
+ /* Not sure if it is a good idea to enable automatic
+ * conversion. Prevent it for now */
+ //bp::to_python_converter< Model,GeometryModelPythonVisitor >();
+ bp::to_python_converter< GeometryModelHandler::SmartPtr_t,GeometryModelPythonVisitor >();
+ }
+
+
+ };
+
+
+
+ }} // namespace se3::python
+
+#endif // ifndef __se3_python_geometry_model_hpp__
+
diff --git a/src/python/parsers.hpp b/src/python/parsers.hpp
index b408a54b644972cf125600fa2ab4671a01509a1a..550f47f25be070e579f105951db0d6820118d530 100644
--- a/src/python/parsers.hpp
+++ b/src/python/parsers.hpp
@@ -26,6 +26,11 @@
#ifdef WITH_URDFDOM
#include "pinocchio/multibody/parser/urdf.hpp"
+#ifdef WITH_HPP_FCL
+ #include "pinocchio/python/geometry-model.hpp"
+ #include "pinocchio/python/geometry-data.hpp"
+ #include "pinocchio/multibody/parser/urdf-with-geometry.hpp"
+#endif
#endif
#ifdef WITH_LUA
@@ -38,6 +43,14 @@ namespace se3
{
struct ParsersPythonVisitor
{
+
+ template<class T1, class T2>
+ struct PairToTupleConverter {
+ static PyObject* convert(const std::pair<T1, T2>& pair) {
+ return boost::python::incref(boost::python::make_tuple(pair.first, pair.second).ptr());
+ }
+ };
+
#ifdef WITH_URDFDOM
struct build_model_visitor : public boost::static_visitor<ModelHandler>
@@ -68,6 +81,57 @@ namespace se3
*model = se3::urdf::buildModel(filename);
return ModelHandler(model,true);
}
+
+
+#ifdef WITH_HPP_FCL
+ struct build_model_and_geom_visitor : public boost::static_visitor<std::pair<ModelHandler, GeometryModelHandler> >
+ {
+ const std::string& _filenameUrdf;
+ const std::string& _filenameMeshRootDir;
+
+ build_model_and_geom_visitor(const std::string& filenameUrdf,
+ const std::string& filenameMeshRootDir): _filenameUrdf(filenameUrdf)
+ , _filenameMeshRootDir(filenameMeshRootDir)
+ {}
+
+ template <typename T> std::pair<ModelHandler, GeometryModelHandler> operator()( T & operand ) const
+ {
+
+
+ Model * model = new Model();
+ GeometryModel * geometry_model = new GeometryModel();
+ std::pair < Model, GeometryModel > models = se3::urdf::buildModelAndGeom(_filenameUrdf, _filenameMeshRootDir, operand);
+ *model = models.first;
+ *geometry_model = models.second;
+ return std::pair<ModelHandler, GeometryModelHandler> ( ModelHandler(model, true),
+ GeometryModelHandler(geometry_model, true)
+ );
+ }
+ };
+
+ static std::pair<ModelHandler, GeometryModelHandler> buildModelAndGeomFromUrdfWithRoot( const std::string & filenameUrdf,
+ const std::string & filenameMeshRootDir,
+ bp::object o
+ )
+ {
+ JointModelVariant variant = bp::extract<JointModelVariant> (o);
+ return boost::apply_visitor(build_model_and_geom_visitor(filenameUrdf, filenameMeshRootDir), variant);
+ }
+
+ static std::pair<ModelHandler, GeometryModelHandler> buildModelAndGeomFromUrdf( const std::string & filenameUrdf,
+ const std::string & filenameMeshRootDir)
+ {
+ Model * model = new Model();
+ GeometryModel * geometry_model = new GeometryModel();
+ std::pair < Model, GeometryModel > models = se3::urdf::buildModelAndGeom(filenameUrdf, filenameMeshRootDir);
+ *model = models.first;
+ *geometry_model = models.second;
+ return std::pair<ModelHandler, GeometryModelHandler> ( ModelHandler(model, true),
+ GeometryModelHandler(geometry_model, true)
+ );
+ }
+#endif
+
#endif
#ifdef WITH_LUA
@@ -85,17 +149,32 @@ namespace se3
/* --- Expose --------------------------------------------------------- */
static void expose()
{
+#ifdef WITH_URDFDOM
bp::def("buildModelFromUrdfWithRoot",buildModelFromUrdfWithRoot,
bp::args("Filename (string)",
"Root Joint Model"),
"Parse the urdf file given in input and return a proper pinocchio model "
"(remember to create the corresponding data structure).");
-#ifdef WITH_URDFDOM
bp::def("buildModelFromUrdf",buildModelFromUrdf,
bp::args("Filename (string)"),
"Parse the urdf file given in input and return a proper pinocchio model "
"(remember to create the corresponding data structure).");
+ #ifdef WITH_HPP_FCL
+
+ bp::to_python_converter<std::pair<ModelHandler, GeometryModelHandler>, PairToTupleConverter<ModelHandler, GeometryModelHandler> >();
+
+ bp::def("buildModelAndGeomFromUrdfWithRoot",buildModelAndGeomFromUrdfWithRoot,
+ bp::args("FilenameUrdf (string)", "FilenameMeshRootDirhRootDir string) ",
+ "Root Joint Model"),
+ "Parse the urdf file given in input and return a proper pinocchio model starting with a given root joint and geometry model "
+ "(remember to create the corresponding data structures).");
+
+ bp::def("buildModelAndGeomFromUrdf",buildModelAndGeomFromUrdf,
+ bp::args("Filename (string)"), "FilenameMeshRootDirhRootDir string) ",
+ "Parse the urdf file given in input and return a proper pinocchio model and geometry model"
+ "(remember to create the corresponding data structures).");
+ #endif
#endif
#ifdef WITH_LUA
diff --git a/src/python/python.cpp b/src/python/python.cpp
index a95464632e37c956bf978d806e97ce4ae464d15a..eddca903957f9c6fcdbc6d933f3c858721a7c6bc 100644
--- a/src/python/python.cpp
+++ b/src/python/python.cpp
@@ -30,6 +30,11 @@
#include "pinocchio/python/parsers.hpp"
#include "pinocchio/python/explog.hpp"
+#ifdef WITH_HPP_FCL
+#include "pinocchio/python/geometry-model.hpp"
+#include "pinocchio/python/geometry-data.hpp"
+#endif
+
namespace se3
{
namespace python
@@ -62,6 +67,10 @@ namespace se3
{
ModelPythonVisitor::expose();
DataPythonVisitor::expose();
+#ifdef WITH_HPP_FCL
+ GeometryModelPythonVisitor::expose();
+ GeometryDataPythonVisitor::expose();
+#endif
}
void exposeAlgorithms()
{
diff --git a/src/spatial/fcl-pinocchio-conversions.hpp b/src/spatial/fcl-pinocchio-conversions.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..ae11ac8bf726807e991912bfbab5e2ef2a32cc8b
--- /dev/null
+++ b/src/spatial/fcl-pinocchio-conversions.hpp
@@ -0,0 +1,71 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_fcl_convertion_hpp__
+#define __se3_fcl_convertion_hpp__
+
+#include <Eigen/Geometry>
+
+#include "pinocchio/spatial/se3.hpp"
+# include <hpp/fcl/math/transform.h>
+
+namespace se3
+{
+
+ inline fcl::Matrix3f toFclMatrix3f(const Eigen::Matrix3d & mat)
+ {
+ return fcl::Matrix3f( mat(0,0),mat(0,1), mat(0,2),
+ mat(1,0),mat(1,1), mat(1,2),
+ mat(2,0),mat(2,1), mat(2,2));
+ }
+
+ inline Eigen::Matrix3d toMatrix3d(const fcl::Matrix3f & mat)
+ {
+ Eigen::Matrix3d res;
+
+ res << mat(0,0),mat(0,1), mat(0,2),
+ mat(1,0),mat(1,1), mat(1,2),
+ mat(2,0),mat(2,1), mat(2,2);
+ return res;
+ }
+
+ inline fcl::Vec3f toFclVec3f(const Eigen::Vector3d & vec)
+ {
+ return fcl::Vec3f( vec(0),vec(1), vec(2));
+ }
+
+ inline Eigen::Vector3d toVector3d(const fcl::Vec3f & vec)
+ {
+ Eigen::Vector3d res;
+ res << vec[0],vec[1], vec[2];
+ return res;
+ }
+
+ fcl::Transform3f toFclTransform3f(const se3::SE3 & m)
+ {
+ return fcl::Transform3f(toFclMatrix3f(m.rotation()), toFclVec3f(m.translation()));
+ }
+
+ se3::SE3 toPinocchioSE3(const fcl::Transform3f & tf)
+ {
+ return se3::SE3(toMatrix3d(tf.getRotation()), toVector3d(tf.getTranslation()));
+ }
+
+} // namespace se3
+
+#endif // ifndef __se3_fcl_convertion_hpp__
+
diff --git a/src/spatial/inertia.hpp b/src/spatial/inertia.hpp
index 7427c62cc345d9ab9e6082db607fdafdbd8ef8d7..715ef31f83c66c47be5a12a35f595cd7aa6529a5 100644
--- a/src/spatial/inertia.hpp
+++ b/src/spatial/inertia.hpp
@@ -55,7 +55,7 @@ namespace se3
Derived_t& operator= (const Derived_t& clone){return derived().__equl__(clone);}
- Derived_t& operator== (const Derived_t& other){return derived().isEqual(other);}
+ bool operator== (const Derived_t& other) const {return derived().isEqual(other);}
Derived_t& operator+= (const Derived_t & Yb) { return derived().__pequ__(Yb); }
Derived_t operator+(const Derived_t & Yb) const { return derived().__plus__(Yb); }
Force operator*(const Motion & v) const { return derived().__mult__(v); }
@@ -204,7 +204,7 @@ namespace se3
}
// Requiered by std::vector boost::python bindings.
- bool isEqual( const InertiaTpl& Y2 )
+ bool isEqual( const InertiaTpl& Y2 ) const
{
return (m==Y2.m) && (c==Y2.c) && (I==Y2.I);
}
diff --git a/src/spatial/se3.hpp b/src/spatial/se3.hpp
index 9743af0ce1f6fa3441285cd6d3d5fedce9ed791b..5d5b7cd430a276e75dc3325ff3598bc04497b905 100644
--- a/src/spatial/se3.hpp
+++ b/src/spatial/se3.hpp
@@ -78,6 +78,7 @@ namespace se3
operator Matrix6() const { return toActionMatrix(); }
+
void disp(std::ostream & os) const
{
static_cast<const Derived_t*>(this)->disp_impl(os);
@@ -102,6 +103,17 @@ namespace se3
Derived_t act (const Derived_t& m2) const { return derived().act_impl(m2); }
Derived_t actInv(const Derived_t& m2) const { return derived().actInv_impl(m2); }
+
+ bool operator == (const Derived_t & other) const
+ {
+ return derived().__equal__(other);
+ }
+
+ bool isApprox (const Derived_t & other) const
+ {
+ return derived().isApprox_impl(other);
+ }
+
friend std::ostream & operator << (std::ostream & os,const SE3Base<Derived> & X)
{
X.disp(os);
@@ -153,12 +165,19 @@ namespace se3
{
}
+ template<typename M4>
+ SE3Tpl(const Eigen::MatrixBase<M4> & m)
+ : rot(m.template block<3,3>(LINEAR,LINEAR)), trans(m.template block<3,1>(LINEAR,ANGULAR))
+ {
+ }
+
SE3Tpl(int) : rot(Matrix3::Identity()), trans(Vector3::Zero()) {}
template<typename S2, int O2>
SE3Tpl( const SE3Tpl<S2,O2> & clone )
: rot(clone.rotation()),trans(clone.translation()) {}
+
template<typename S2, int O2>
SE3Tpl & operator= (const SE3Tpl<S2,O2> & other)
{
@@ -220,6 +239,7 @@ namespace se3
return M;
}
+
void disp_impl(std::ostream & os) const
{
os << " R =\n" << rot << std::endl
@@ -249,6 +269,18 @@ namespace se3
SE3Tpl __mult__(const SE3Tpl & m2) const { return this->act(m2);}
+ bool __equal__( const SE3Tpl & m2 ) const
+ {
+ return (rotation_impl() == m2.rotation() && translation_impl() == m2.translation());
+ }
+
+ bool isApprox_impl( const SE3Tpl & m2 ) const
+ {
+ Matrix4 diff( toHomogeneousMatrix_impl() -
+ m2.toHomogeneousMatrix_impl());
+ return (diff.isMuchSmallerThan(toHomogeneousMatrix_impl(), 1e-14)
+ && diff.isMuchSmallerThan(m2.toHomogeneousMatrix_impl(), 1e-14) );
+ }
public:
const Angular_t & rotation_impl() const { return rot; }
diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt
index 1716233faa8049cd871770d61c09a4d72355904f..088db0a40ca5277b4f962c6cee5da0edee8a32c5 100644
--- a/unittest/CMakeLists.txt
+++ b/unittest/CMakeLists.txt
@@ -61,6 +61,18 @@ IF(URDFDOM_FOUND)
ADD_UNIT_TEST(value "eigen3;urdfdom")
ADD_TEST_CFLAGS(value '-DPINOCCHIO_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
+ IF(HPP_FCL_FOUND)
+ ADD_UNIT_TEST(geom "eigen3;urdfdom;assimp;hpp-fcl")
+ ADD_TEST_CFLAGS(geom '-DPINOCCHIO_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
+ ADD_TEST_CFLAGS(geom "-DWITH_HPP_FCL")
+ IF(BUILD_TESTS_WITH_HPP)
+ ADD_OPTIONAL_DEPENDENCY("hpp-model-urdf")
+ IF(HPP_MODEL_URDF_FOUND)
+ PKG_CONFIG_USE_DEPENDENCY(geom hpp-model-urdf)
+ ADD_TEST_CFLAGS(geom "-DWITH_HPP_MODEL_URDF")
+ ENDIF(HPP_MODEL_URDF_FOUND)
+ ENDIF(BUILD_TESTS_WITH_HPP)
+ ENDIF(HPP_FCL_FOUND)
ENDIF(URDFDOM_FOUND)
IF(LUA5_1_FOUND)
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4d211e9b8a470a991a34b7c845e26916c5254956
--- /dev/null
+++ b/unittest/geom.cpp
@@ -0,0 +1,526 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+/*
+ * Compare the value obtained with the RNEA with the values obtained from
+ * RBDL. The test is not complete. It only validates the RNEA for the revolute
+ * joints. The free-flyer is not tested. It should be extended to account for
+ * the free flyer and for the other algorithms.
+ *
+ * Additionnal notes: the RNEA is an algorithm that can be used to validate
+ * many others (in particular, the mass matrix (CRBA) can be numerically
+ * validated from the RNEA, then the center-of-mass jacobian can be validated
+ * from the mass matrix, etc.
+ *
+ */
+
+#include <iostream>
+#include <iomanip>
+
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/collisions.hpp"
+#include "pinocchio/multibody/parser/urdf.hpp"
+#include "pinocchio/spatial/explog.hpp"
+
+#include "pinocchio/multibody/geometry.hpp"
+#include "pinocchio/multibody/parser/urdf-with-geometry.hpp"
+
+#include <vector>
+
+#ifdef WITH_HPP_MODEL_URDF
+#include <hpp/util/debug.hh>
+#include <hpp/model/device.hh>
+#include <hpp/model/body.hh>
+#include <hpp/model/collision-object.hh>
+#include <hpp/model/joint.hh>
+#include <hpp/model/urdf/util.hh>
+#endif
+
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE GeomTest
+#include <boost/test/unit_test.hpp>
+#include "pinocchio/tools/matrix-comparison.hpp"
+
+typedef std::map <std::string, se3::SE3> PositionsMap_t;
+typedef std::map <std::string, se3::SE3> JointPositionsMap_t;
+typedef std::map <std::string, se3::SE3> GeometryPositionsMap_t;
+typedef std::map <std::pair < std::string , std::string >, fcl::DistanceResult > PairDistanceMap_t;
+JointPositionsMap_t fillPinocchioJointPositions(const se3::Data & data);
+GeometryPositionsMap_t fillPinocchioGeometryPositions(const se3::GeometryData & data_geom);
+#ifdef WITH_HPP_MODEL_URDF
+JointPositionsMap_t fillHppJointPositions(const hpp::model::HumanoidRobotPtr_t robot);
+GeometryPositionsMap_t fillHppGeometryPositions(const hpp::model::HumanoidRobotPtr_t robot);
+#endif
+std::vector<std::string> getBodiesList();
+
+#ifdef WITH_HPP_MODEL_URDF
+class IsDistanceResultPair
+{
+typedef std::string string;
+public:
+IsDistanceResultPair( string body1, string body2): _body1(body1), _body2(body2){}
+
+bool operator()(hpp::model::DistanceResult dist) const
+{
+ return (dist.innerObject->name() == _body1 && dist.outerObject->name() == _body2);
+}
+private:
+string _body1;
+string _body2;
+};
+
+struct Distance_t
+{
+ Distance_t () :d_ (0), x0_ (0), y0_ (0), z0_ (0), x1_ (0), y1_ (0), z1_ (0)
+ {
+ }
+ Distance_t (double d, double x0, double y0, double z0,
+ double x1, double y1, double z1) : d_ (d),
+ x0_ (x0), y0_ (y0), z0_ (z0),
+ x1_ (x1), y1_ (y1), z1_ (z1)
+ {
+ }
+ Distance_t ( fcl::DistanceResult dr) : d_ (dr.min_distance),
+ x0_ (dr.nearest_points [0][0]), y0_ (dr.nearest_points [0][1]), z0_ (dr.nearest_points [0][2]),
+ x1_ (dr.nearest_points [1][0]), y1_ (dr.nearest_points [1][1]), z1_ (dr.nearest_points [1][2])
+ {
+ }
+ Distance_t (const Distance_t& other) :
+ d_ (other.d_), x0_ (other.x0_), y0_ (other.y0_), z0_ (other.z0_),
+ x1_ (other.x1_), y1_ (other.y1_), z1_ (other.z1_)
+ {
+ }
+ Distance_t& swap ()
+ {
+ double tmp;
+ tmp = x1_; x1_ = x0_; x0_ = tmp;
+ tmp = y1_; y1_ = y0_; y0_ = tmp;
+ tmp = z1_; z1_ = z0_; z0_ = tmp;
+ return *this;
+ }
+ void checkClose (const Distance_t& other)
+ {
+ BOOST_CHECK_MESSAGE (fabs (d_ - other.d_ ) < 1e-5, "values d are " << d_ << " and " << other.d_);
+ BOOST_CHECK_MESSAGE (fabs (x0_ - other.x0_ ) < 1e-5, "values x0 are " << x0_ << " and " << other.x0_);
+ BOOST_CHECK_MESSAGE (fabs (y0_ - other.y0_ ) < 1e-5, "values y0 are " << y0_ << " and " << other.y0_);
+ BOOST_CHECK_MESSAGE (fabs (z0_ - other.z0_ ) < 1e-5, "values z0 are " << z0_ << " and " << other.z0_);
+ BOOST_CHECK_MESSAGE (fabs (x1_ - other.x1_ ) < 1e-5, "values x1 are " << x1_ << " and " << other.x1_);
+ BOOST_CHECK_MESSAGE (fabs (y1_ - other.y1_ ) < 1e-5, "values y1 are " << y1_ << " and " << other.y1_);
+ BOOST_CHECK_MESSAGE (fabs (z1_ - other.z1_ ) < 1e-5, "values z1 are " << z1_ << " and " << other.z1_);
+ }
+ // Distance and closest points on bodies.
+ double d_, x0_, y0_, z0_, x1_, y1_, z1_;
+}; // struct Distance_t
+#endif
+std::ostream& operator<<(std::ostream& os, const std::pair < se3::Model, se3::GeometryModel >& robot)
+{
+ os << "Nb collision objects = " << robot.second.ngeom << std::endl;
+
+ for(se3::GeometryModel::Index i=0;i<(se3::GeometryModel::Index)(robot.second.ngeom);++i)
+ {
+ os << "Object n " << i << " : " << robot.second.geom_names[i] << ": attached to joint = " << robot.second.geom_parents[i]
+ << "=" << robot.first.getJointName(robot.second.geom_parents[i]) << std::endl;
+ }
+ return os;
+}
+
+
+BOOST_AUTO_TEST_SUITE ( GeomTest )
+
+BOOST_AUTO_TEST_CASE ( simple_boxes )
+{
+ se3::Model model;
+ se3::GeometryModel model_geom;
+
+ se3::buildModels::collisionModel(model, model_geom);
+
+ se3::Data data(model);
+ se3::GeometryData data_geom(data, model_geom);
+
+ std::cout << "------ Model ------ " << std::endl;
+ std::cout << model;
+ std::cout << "------ Geom ------ " << std::endl;
+ std::cout << model_geom;
+ std::cout << "------ DataGeom ------ " << std::endl;
+ std::cout << data_geom;
+ assert(data_geom.collide(0,1) == true && "");
+
+ Eigen::VectorXd q(model.nq);
+ q << 2, 0, 0,
+ 0, 0, 0 ;
+
+ se3::updateCollisionGeometry(model, data, model_geom, data_geom, q, true);
+ std::cout << data_geom;
+ assert(data_geom.collide(0,1) == false && "");
+
+ q << 0.99, 0, 0,
+ 0, 0, 0 ;
+
+ se3::updateCollisionGeometry(model, data, model_geom, data_geom, q, true);
+ std::cout << data_geom;
+ assert(data_geom.collide(0,1) == true && "");
+
+ q << 1.01, 0, 0,
+ 0, 0, 0 ;
+
+ se3::updateCollisionGeometry(model, data, model_geom, data_geom, q, true);
+ std::cout << data_geom;
+ assert(data_geom.collide(0,1) == false && "");
+}
+
+BOOST_AUTO_TEST_CASE ( loading_model )
+{
+ typedef se3::Model Model;
+ typedef se3::GeometryModel GeometryModel;
+ typedef se3::Data Data;
+ typedef se3::GeometryData GeometryData;
+
+
+ std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+ std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/";
+ std::pair < Model, GeometryModel > robot = se3::urdf::buildModelAndGeom(filename, meshDir, se3::JointModelFreeFlyer());
+ std::cout << robot.first << std::endl;
+
+ Data data(robot.first);
+ GeometryData data_geom(data, robot.second);
+
+ Eigen::VectorXd q(robot.first.nq);
+ q << 0, 0, 0.840252, 0, 0, 0, 1, 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, 0, -0.3490658,
+ 0.6981317, -0.3490658, 0, 0, 1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, 0, 0, 0, 0,
+ 1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2 ;
+
+ se3::updateCollisionGeometry(robot.first, data, robot.second, data_geom, q, true);
+
+ assert(data_geom.collide(1,10) == false && "");
+}
+
+#ifdef WITH_HPP_MODEL_URDF
+BOOST_AUTO_TEST_CASE ( hrp2_joints_meshes_positions )
+{
+ typedef se3::Model Model;
+ typedef se3::GeometryModel GeometryModel;
+ typedef se3::Data Data;
+ typedef se3::GeometryData GeometryData;
+ using hpp::model::Device;
+ using hpp::model::Joint;
+ using hpp::model::Body;
+ typedef hpp::model::ObjectVector_t ObjectVector_t;
+ typedef hpp::model::JointVector_t JointVector_t;
+ typedef std::vector<double> vector_t;
+
+
+
+ /// ********** Pinocchio ********** ///
+ /// ********************************* ///
+
+ // Building the model in pinocchio and compute kinematics/geometry for configuration q_pino
+ std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+ std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/";
+ std::pair < Model, GeometryModel > robot = se3::urdf::buildModelAndGeom(filename, meshDir, se3::JointModelFreeFlyer());
+
+ Data data(robot.first);
+ GeometryData data_geom(data, robot.second);
+
+ // Configuration to be tested
+
+
+ Eigen::VectorXd q_pino(Eigen::VectorXd::Random(robot.first.nq));
+ q_pino.segment<4>(3) /= q_pino.segment<4>(3).norm();
+
+ Eigen::VectorXd q_hpp(q_pino);
+ Eigen::Vector4d quaternion;
+ quaternion << q_pino[6], q_pino[3], q_pino[4], q_pino[5];
+ q_hpp.segment<4>(3) = quaternion ;
+
+
+ assert(q_pino.size() == robot.first.nq && "wrong config size");
+
+ se3::updateCollisionGeometry(robot.first, data, robot.second, data_geom, q_pino, true);
+
+
+ /// ************* HPP ************* ///
+ /// ********************************* ///
+
+
+ hpp::model::HumanoidRobotPtr_t humanoidRobot =
+ hpp::model::HumanoidRobot::create ("romeo");
+ hpp::model::urdf::loadHumanoidModel(humanoidRobot, "freeflyer",
+ "romeo_pinocchio", "romeo",
+ "", "");
+
+
+ assert(robot.first.nq == humanoidRobot->configSize () && "Pinocchio model & HPP model config sizes are not the same ");
+
+ humanoidRobot->currentConfiguration (q_hpp);
+ humanoidRobot->computeForwardKinematics ();
+
+
+
+ /// ********** COMPARISON ********* ///
+ /// ********************************* ///
+ // retrieve all joint and geometry objects positions
+ JointPositionsMap_t joints_pin = fillPinocchioJointPositions(data);
+ JointPositionsMap_t joints_hpp = fillHppJointPositions(humanoidRobot);
+ GeometryPositionsMap_t geom_pin = fillPinocchioGeometryPositions(data_geom);
+ GeometryPositionsMap_t geom_hpp = fillHppGeometryPositions(humanoidRobot);
+
+
+ std::map <std::string, se3::SE3>::iterator it_hpp;
+ std::map <std::string, se3::SE3>::iterator it_pin;
+
+ // Comparing position of joints
+ se3::SE3 ff_pin = joints_pin["root_joint"];
+ se3::SE3 ff_hpp = joints_hpp["base_joint_SO3"];
+ assert( ff_pin.isApprox(ff_hpp) && "ff_hpp and ff_pin are not ==");
+
+ for (it_pin = joints_pin.begin(); it_pin != joints_pin.end();
+ ++it_pin)
+ {
+ it_hpp = joints_hpp.find(it_pin->first);
+ if (it_hpp != joints_hpp.end())
+ {
+ assert(it_pin->second.isApprox(it_hpp->second) && "joint positions are not equal");
+ // se3::Motion nu = se3::log6(it_pin->second.inverse() * it_hpp->second);
+ }
+ }
+
+ // Comparing position of geometry objects
+ for (it_pin = geom_pin.begin(); it_pin != geom_pin.end();
+ ++it_pin)
+ {
+ it_hpp = geom_hpp.find(it_pin->first);
+ if (it_hpp != joints_hpp.end())
+ {
+ assert(it_pin->second.isApprox(it_hpp->second) && "geometry objects positions are not equal");
+ }
+ }
+
+
+}
+
+
+BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
+{
+ typedef se3::Model Model;
+ typedef se3::GeometryModel GeometryModel;
+ typedef se3::Data Data;
+ typedef se3::GeometryData GeometryData;
+ using hpp::model::Device;
+ using hpp::model::Joint;
+ using hpp::model::Body;
+ typedef hpp::model::ObjectVector_t ObjectVector_t;
+ typedef hpp::model::JointVector_t JointVector_t;
+ typedef std::vector<double> vector_t;
+
+
+
+ /// ********** Pinocchio ********** ///
+ /// ********************************* ///
+
+ // Building the model in pinocchio and compute kinematics/geometry for configuration q_pino
+ std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+ std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/";
+ std::pair < Model, GeometryModel > robot = se3::urdf::buildModelAndGeom(filename, meshDir, se3::JointModelFreeFlyer());
+
+ Data data(robot.first);
+ GeometryData data_geom(data, robot.second);
+
+ // Configuration to be tested
+
+
+ Eigen::VectorXd q_pino(Eigen::VectorXd::Random(robot.first.nq));
+ q_pino.segment<4>(3) /= q_pino.segment<4>(3).norm();
+
+ Eigen::VectorXd q_hpp(q_pino);
+ Eigen::Vector4d quaternion;
+ quaternion << q_pino[6], q_pino[3], q_pino[4], q_pino[5];
+ q_hpp.segment<4>(3) = quaternion ;
+
+
+ assert(q_pino.size() == robot.first.nq && "wrong config size");
+
+ se3::updateCollisionGeometry(robot.first, data, robot.second, data_geom, q_pino, true);
+
+
+ /// ************* HPP ************* ///
+ /// ********************************* ///
+
+
+ hpp::model::HumanoidRobotPtr_t humanoidRobot =
+ hpp::model::HumanoidRobot::create ("romeo");
+ hpp::model::urdf::loadHumanoidModel(humanoidRobot, "freeflyer",
+ "romeo_pinocchio", "romeo",
+ "", "");
+
+
+ assert(robot.first.nq == humanoidRobot->configSize () && "Pinocchio model & HPP model config sizes are not the same ");
+
+ humanoidRobot->currentConfiguration (q_hpp);
+ humanoidRobot->computeForwardKinematics ();
+
+
+ humanoidRobot->computeDistances ();
+
+ /// ********** COMPARISON ********* ///
+ /// ********************************* ///
+
+ const hpp::model::DistanceResults_t& distances(humanoidRobot->distanceResults ());
+ std::vector<std::string> bodies = getBodiesList();
+
+ for (std::vector<std::string>::iterator i = bodies.begin(); i != bodies.end(); ++i)
+ {
+ for (std::vector<std::string>::iterator j = bodies.begin(); j != bodies.end(); ++j)
+ {
+ std::string body1 (*i); std::string inner1(body1 + "_0");
+ std::string body2 (*j); std::string inner2(body2 + "_0");
+ hpp::model::DistanceResults_t::const_iterator it = std::find_if ( distances.begin(),
+ distances.end(),
+ IsDistanceResultPair(inner1, inner2)
+ );
+ if(it != distances .end())
+ {
+ Distance_t distance_hpp(it->fcl);
+
+
+ std::cout << "comparison between " << body1 << " and " << body2 << std::endl;
+
+ fcl::DistanceResult dist_pin = data_geom.computeDistance( robot.second.getGeomId(body1),
+ robot.second.getGeomId(body2));
+
+ Distance_t distance_pin ( dist_pin);
+ distance_hpp.checkClose(distance_pin);
+ }
+ }
+ }
+
+}
+#endif
+BOOST_AUTO_TEST_SUITE_END ()
+
+JointPositionsMap_t fillPinocchioJointPositions(const se3::Data & data)
+{
+ JointPositionsMap_t result;
+ for (int i = 0; i < data.model.nbody; ++i)
+ {
+ result[data.model.getJointName(i)] = data.oMi[i];
+ }
+ return result;
+}
+
+GeometryPositionsMap_t fillPinocchioGeometryPositions(const se3::GeometryData & data_geom)
+{
+ GeometryPositionsMap_t result;
+ for (std::size_t i = 0; i < data_geom.model_geom.ngeom ; ++i)
+ {
+ result[data_geom.model_geom.getGeomName(i)] = data_geom.oMg[i];
+ }
+ return result;
+}
+
+#ifdef WITH_HPP_MODEL_URDF
+JointPositionsMap_t fillHppJointPositions(const hpp::model::HumanoidRobotPtr_t robot)
+{
+ JointPositionsMap_t result;
+ const hpp::model::JointVector_t& joints(robot->getJointVector ());
+
+ for (hpp::model::JointVector_t::const_iterator it = joints.begin (); it != joints.end (); ++it)
+ {
+ if((*it)->configSize() != 0) // Retrieving joints that are not anchors
+ {
+ result[(*it)->name()] = se3::toPinocchioSE3((*it)->currentTransformation() * (*it)->linkInJointFrame());
+ }
+ }
+ return result;
+
+}
+
+GeometryPositionsMap_t fillHppGeometryPositions(const hpp::model::HumanoidRobotPtr_t robot)
+{
+ GeometryPositionsMap_t result;
+
+ const hpp::model::JointVector_t& joints(robot->getJointVector ());
+ // retrieving only the positions of link that have collision geometry
+ for (hpp::model::JointVector_t::const_iterator it = joints.begin (); it != joints.end (); ++it)
+ {
+ if((*it)->configSize() != 0) // Retrieving body attached to joints that are not anchors
+ {
+ hpp::model::BodyPtr_t body = (*it)->linkedBody ();
+ if(body)
+ {
+ const hpp::model::ObjectVector_t& ov (body->innerObjects (hpp::model::COLLISION));
+ for (hpp::model::ObjectVector_t::const_iterator itObj = ov.begin ();itObj != ov.end (); itObj ++)
+ {
+ result[body->name()] = se3::toPinocchioSE3((*itObj)->fcl ()->getTransform ());
+ }
+ }
+ }
+ }
+
+ return result;
+}
+
+#endif
+
+std::vector<std::string> getBodiesList()
+{
+ std::vector<std::string> result;
+
+ result.push_back( "CHEST_LINK0");
+ result.push_back( "torso");
+ result.push_back( "HEAD_LINK0");
+ result.push_back( "HEAD_LINK1");
+ result.push_back( "LARM_LINK0");
+ result.push_back( "LARM_LINK1");
+ result.push_back( "LARM_LINK2");
+ result.push_back( "LARM_LINK3");
+ result.push_back( "LARM_LINK4");
+ result.push_back( "l_wrist");
+ result.push_back( "LARM_LINK6");
+ result.push_back( "LHAND_LINK0");
+ result.push_back( "LHAND_LINK1");
+ result.push_back( "LHAND_LINK2");
+ result.push_back( "LHAND_LINK3");
+ result.push_back( "LHAND_LINK4");
+ result.push_back( "RARM_LINK0");
+ result.push_back( "RARM_LINK1");
+ result.push_back( "RARM_LINK2");
+ result.push_back( "RARM_LINK3");
+ result.push_back( "RARM_LINK4");
+ result.push_back( "r_wrist");
+ result.push_back( "RARM_LINK6");
+ result.push_back( "RHAND_LINK0");
+ result.push_back( "RHAND_LINK1");
+ result.push_back( "RHAND_LINK2");
+ result.push_back( "RHAND_LINK3");
+ result.push_back( "RHAND_LINK4");
+ result.push_back( "LLEG_LINK0");
+ result.push_back( "LLEG_LINK1");
+ result.push_back( "LLEG_LINK2");
+ result.push_back( "LLEG_LINK3");
+ result.push_back( "LLEG_LINK4");
+ result.push_back( "l_ankle");
+ result.push_back( "RLEG_LINK0");
+ result.push_back( "RLEG_LINK1");
+ result.push_back( "RLEG_LINK2");
+ result.push_back( "RLEG_LINK3");
+ result.push_back( "RLEG_LINK4");
+ result.push_back( "r_ankle");
+
+ return result;
+}