diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e3b2858b257f4f1a45531c112e7c67dcea91bfea
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,57 @@
+# Copyright (c) 2012 CNRS
+# Author: Florent Lamiraux
+#
+# This file is part of hpp-rbprm-corba.
+# hpp-rbprm-corba 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.
+#
+# hpp-rbprm-corba 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
+# hpp-rbprm-corba. If not, see
+# <http://www.gnu.org/licenses/>.
+
+# Requires at least CMake 2.6 to configure the package.
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+
+SET(CXX_DISABLE_WERROR true)
+
+INCLUDE(cmake/base.cmake)
+INCLUDE(cmake/idl.cmake)
+INCLUDE(cmake/python.cmake)
+
+SET(PROJECT_NAME hpp-rbprm-corba)
+SET(PROJECT_DESCRIPTION "Corba server for reachability based planning")
+SET(PROJECT_URL "")
+
+SET(CUSTOM_HEADER_DIR hpp/corbaserver/rbprm)
+
+SETUP_PROJECT ()
+
+SET(${PROJECT_NAME}_HEADERS
+ include/hpp/corbaserver/rbprm/server.hh
+ include/hpp/corbaserver/rbprm/fwd.hh
+)
+
+# Activate hpp-util logging if requested
+SET (HPP_DEBUG FALSE CACHE BOOL "trigger hpp-util debug output")
+IF (HPP_DEBUG)
+ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHPP_DEBUG")
+ENDIF()
+
+ADD_DOC_DEPENDENCY("hpp-core >= 3")
+ADD_REQUIRED_DEPENDENCY("hpp-corbaserver >= 3")
+ADD_REQUIRED_DEPENDENCY("hpp-rbprm")
+ADD_REQUIRED_DEPENDENCY("omniORB4 >= 4.1.4")
+
+PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
+ADD_SUBDIRECTORY(src)
+
+CONFIG_FILES (include/hpp/corbaserver/rbprm/doc.hh)
+
+SETUP_PROJECT_FINALIZE()
+
diff --git a/build/src/hpp/corbaserver/rbprm/__init__.py b/build/src/hpp/corbaserver/rbprm/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..3f7040918ca270692e5a7bbacce0975e2c5ecf89
--- /dev/null
+++ b/build/src/hpp/corbaserver/rbprm/__init__.py
@@ -0,0 +1,13 @@
+# DO NOT EDIT THIS FILE!
+#
+# Python module hpp.corbaserver.rbprm generated by omniidl
+
+import omniORB
+omniORB.updateModule("hpp.corbaserver.rbprm")
+
+# ** 1. Stub files contributing to this module
+import rbprmbuilder_idl
+
+# ** 2. Sub-modules
+
+# ** 3. End
diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in
new file mode 100644
index 0000000000000000000000000000000000000000..5d5bde768fa159f22fdc44b86197e20adf77d7d8
--- /dev/null
+++ b/doc/Doxyfile.extra.in
@@ -0,0 +1,13 @@
+TAGFILES = @HPP_CORE_DOXYGENDOCDIR@/hpp-core.doxytag=@HPP_CORE_DOXYGENDOCDIR@ \
+TAGFILES = @HPP_CORBASERVER_DOXYGENDOCDIR@/hpp-corbaserver.doxytag=@HPP_CORBASERVER_DOXYGENDOCDIR@ \
+ @HPP_RBPRM_DOXYGENDOCDIR@/hpp-rbprm.doxytag=@HPP_RBPRM_DOXYGENDOCDIR@
+
+INPUT = @CMAKE_SOURCE_DIR@/include \
+ @CMAKE_BINARY_DIR@/include \
+ @CMAKE_SOURCE_DIR@/idl \
+ @CMAKE_SOURCE_DIR@/src/hpp/corbaserver/rbprm/rbprmbuilder.py \
+
+
+FILE_PATTERNS = *.cc *.cpp \
+ *.hh *.hxx \
+ *.idl *.py
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
new file mode 100644
index 0000000000000000000000000000000000000000..e8638f24929656c4163b38ae84b2965ff7b918af
--- /dev/null
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -0,0 +1,75 @@
+// Copyright (c) 2015 CNRS
+// Author: Steve Tonneau
+//
+// This file is part of hpp-manipulation-corba.
+// hpp-manipulation-corba 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.
+//
+// hpp-manipulation-corba 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
+// hpp-manipulation-corba. If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_MANIPULATION_CORBA_ROBOT_IDL
+# define HPP_MANIPULATION_CORBA_ROBOT_IDL
+
+# include <hpp/corbaserver/common.idl>
+
+module hpp
+{
+ module corbaserver {
+ module rbprm {
+ interface RbprmBuilder
+ {
+ /// Create a Device for the ROM of the robot
+
+ /// The device automatically has an anchor joint called "base_joint" as
+ /// root joint.
+ /// \param romRobotName the name of the robot range of motion.
+ /// Load robot model
+ /// \param rootJointType type of root joint among "anchor", "freeflyer",
+ /// "planar",
+ /// \param packageName Name of the ROS package containing the model,
+ /// \param modelName Name of the package containing the model
+ /// \param urdfSuffix suffix for urdf file,
+ ///
+ /// The ros url are built as follows:
+ /// "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
+ /// "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
+ ///
+ void loadRobotRomModel (in string romRobotName, in string rootJointType,
+ in string packageName, in string modelName,
+ in string urdfSuffix, in string srdfSuffix)
+ raises (Error);
+
+
+ /// Create a RbprmDevice for the robot of the robot
+
+ /// The device automatically has an anchor joint called "base_joint" as
+ /// root joint.
+ /// \param trunkRobotName the name of the robot trunk used for collision.
+ /// \param rootJointType type of root joint among "anchor", "freeflyer",
+ /// "planar",
+ /// \param packageName Name of the ROS package containing the model,
+ /// \param modelName Name of the package containing the model
+ /// \param urdfSuffix suffix for urdf file,
+ ///
+ /// The ros url are built as follows:
+ /// "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
+ /// "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
+ ///
+ void loadRobotCompleteModel (in string trunkRobotName, in string rootJointType,
+ in string packageName, in string modelName,
+ in string urdfSuffix, in string srdfSuffix)
+ raises (Error);
+ }; // interface Robot
+ }; // module rbprm
+ }; // module corbaserver
+}; // module hpp
+
+#endif // HPP_MANIPULATION_CORBA_ROBOT_IDL
diff --git a/include/hpp/corbaserver/rbprm/doc.hh.in b/include/hpp/corbaserver/rbprm/doc.hh.in
new file mode 100644
index 0000000000000000000000000000000000000000..4f0b6513cf432348682690438d7ef8353b0c0b9f
--- /dev/null
+++ b/include/hpp/corbaserver/rbprm/doc.hh.in
@@ -0,0 +1,84 @@
+// Copyright (C) 2014 CNRS-LAAS
+// Author: Florent Lamiraux.
+//
+// This file is part of the hpp-manipulation-corba.
+//
+// hpp-manipulation-corba 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.
+//
+// hpp-manipulation-corba 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License along with hpp-manipulation-corba. If not, see
+// <http://www.gnu.org/licenses/>.
+
+/// \mainpage
+///
+/// \section hpp_manipulation_corba_intro Introduction
+///
+/// This package implements a Corba server running hpp-manipulation
+/// package functions and the corresponding client.
+///
+/// The corba server part is compiled into library
+/// \c libhpp-manipulation-corba.so that implements the following
+/// class:
+/// \li hpp::manipulation::Server.
+/// The corba services provided by the library are described in interface
+/// \li hpp::corbaserver::manipulation::Graph.
+/// \li hpp::corbaserver::manipulation::Problem.
+/// \li hpp::corbaserver::manipulation::Robot.
+///
+/// The client part is provided through python module
+/// \c hpp.corbaserver.manipulation
+///
+/// An executable called \c hpp-manipulation-server is installed.
+/// This executable runs
+/// \li the corba server implemented by library
+/// \c libhpp-manipulation-corba.so, as well as
+/// \li the corba server implemented in \htmlonly<a class="el" href="@HPP_WHOLEBODY_STEP_CORBA_DOXYGENDOCDIR@/index.html">hpp-wholebody-step-corba</a>\endhtmlonly,
+/// \li the corba server implemented in \htmlonly<a class="el" href="@HPP_CORBASERVER_DOXYGENDOCDIR@/index.html">hpp-corbaserver</a>\endhtmlonly.
+///
+/// See these package documentations for details.
+///
+/// \section hpp_manipulation_corba_howto_communicate \
+/// How to communicate with the CORBA server
+///
+/// The easiest way is
+///
+/// to launch \c hpp-manipulation-server executable,
+/// open a python terminal and type:
+/// \code
+/// from hpp.corbaserver.manipulation import Client as ManipulationClient
+/// mcl = ManipulationClient ()
+/// \endcode
+/// Then variable mcl contains a member \c problem that can send
+/// requests to the server. For instance
+///
+/// \code
+/// mcl.problem.loadHumanoidModel ("hrp2_14", "freeflyer",
+/// "hrp2_14_description", "hrp2_14",
+/// "_capsule", "_capsule")
+/// \endcode
+///
+/// Some python classes are provided that embed corba clients and
+/// that forward corba resquest to the server side:
+/// \li manipulation.robot.Robot to load and handle a manipulation robot,
+/// \li manipulation.robot.HumanoidRobot to load and handle a manipulation
+/// robot, containing a humanoid robot at first place,
+/// \li manipulation.problem_solver.ProblemSolver to set and solve a
+/// manipulation planning problem.
+/// \li manipulation.constraint_graph.ConstraintGraph to initialize the
+/// graph of constraint.
+///
+/// \section hpp_manipulation_corba_how_to_embed \
+/// How to embed a server in an application
+///
+/// Embedding the corba server into an application can be done by linking the
+/// application with \c libhpp-manipulation-corba.so in a similar way as
+/// hpp-corbaserver corba server. See documentation of classes
+/// hpp::corbaServer::Server and hpp::manipulation::Server for details.
diff --git a/include/hpp/corbaserver/rbprm/fwd.hh b/include/hpp/corbaserver/rbprm/fwd.hh
new file mode 100644
index 0000000000000000000000000000000000000000..a2e7fc1cd53356287833e42cbc9d5994fc88d54b
--- /dev/null
+++ b/include/hpp/corbaserver/rbprm/fwd.hh
@@ -0,0 +1,38 @@
+// Copyright (C) 2014 CNRS-LAAS
+// Author: Florent Lamiraux.
+//
+// This file is part of the hpp-manipulation-corba.
+//
+// hpp-manipulation-corba 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.
+//
+// hpp-manipulation-corba 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License along with hpp-manipulation-corba. If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_MANIPULATION_CORBA_FWD_HH
+# define HPP_MANIPULATION_CORBA_FWD_HH
+
+# include <hpp/manipulation/fwd.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace impl {
+ typedef manipulation::ProblemSolver ProblemSolver;
+ class Server;
+ typedef core::ConstraintSet ConstraintSet;
+ typedef core::ConstraintSetPtr_t ConstraintSetPtr_t;
+ typedef core::ConfigProjector ConfigProjector;
+ typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
+ } // namespace impl
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_CORBA_FWD_HH
diff --git a/include/hpp/corbaserver/rbprm/server.hh b/include/hpp/corbaserver/rbprm/server.hh
new file mode 100644
index 0000000000000000000000000000000000000000..1cecf9014abc9211c0c35c1679d00fba1ace5d9b
--- /dev/null
+++ b/include/hpp/corbaserver/rbprm/server.hh
@@ -0,0 +1,53 @@
+// Copyright (C) 2014 CNRS-LAAS
+// Author: Florent Lamiraux.
+//
+// This file is part of the hpp-manipulation-corba.
+//
+// hpp-manipulation-corba 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.
+//
+// hpp-manipulation-corba 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License along with hpp-manipulation-corba. If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_RBPRM_CORBA_SERVER_HH
+# define HPP_RBPRM_CORBA_SERVER_HH
+
+# include <hpp/corba/template/server.hh>
+# include <hpp/corbaserver/rbprm/fwd.hh>
+# include <hpp/corbaserver/rbprm/config.hh>
+
+namespace hpp {
+ namespace rbprm {
+ namespace impl {
+ class RbprmBuilder;
+ }
+ class HPP_RBPRM_CORBA_DLLAPI Server
+ {
+ public:
+ Server (int argc, char *argv[], bool multiThread = false,
+ const std::string& poaName = "child");
+ ~Server ();
+ /// Set planner that will be controlled by server
+ void setProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver);
+
+ /// Start corba server
+
+ /// Call hpp::corba::Server <impl::Problem>::startCorbaServer
+ void startCorbaServer(const std::string& contextId,
+ const std::string& contextKind,
+ const std::string& objectId);
+ private:
+ corba::Server <impl::RbprmBuilder>* rbprmBuilder_;
+ }; // class Server
+ } // namespace rbprm
+} // namespace hpp
+
+#endif // HPP_RBPRM_CORBA_SERVER_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a043a3231821adf63a90d6a9ac7783270f54fa7f
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,130 @@
+# Copyright (c) 2012 CNRS
+# Author: Florent Lamiraux
+#
+# This file is part of hpp-rbprm-corba.
+# hpp-rbprm-corba 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.
+#
+# hpp-rbprm-corba 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
+# hpp-rbprm-corba. If not, see
+# <http://www.gnu.org/licenses/>.
+
+SET(LIBRARY_NAME ${PROJECT_NAME})
+SET(IDL_SOURCES
+ rbprmbuilder
+)
+
+SET(HPP_CORBASERVER_IDL_SOURCES
+ common
+)
+
+OMNIIDL_INCLUDE_DIRECTORIES(
+ ${HPP_CORBASERVER_DATAROOTDIR}/idl ${CMAKE_SOURCE_DIR}/idl
+ )
+
+INCLUDE_DIRECTORIES(${CMAKE_BINARY_DIR}/src)
+FOREACH(IDL ${IDL_SOURCES})
+ GENERATE_IDL_FILE (${IDL}
+ ${CMAKE_SOURCE_DIR}/idl/hpp/corbaserver/rbprm
+ )
+ENDFOREACH()
+
+FOREACH(IDL ${HPP_CORBASERVER_IDL_SOURCES})
+ GENERATE_IDL_FILE (${IDL}
+ ${HPP_CORBASERVER_DATAROOTDIR}/idl/hpp/corbaserver
+ )
+ENDFOREACH()
+
+ADD_LIBRARY(${LIBRARY_NAME} SHARED
+ common.hh
+ commonSK.cc
+ rbprmbuilder.hh
+ rbprmbuilderSK.cc
+ rbprmbuilder.impl.cc
+ rbprmbuilder.impl.hh
+ server.cc
+ ${CMAKE_CURRENT_BINARY_DIR}/hpp/corbaserver/rbprm/rbprmbuilder_idl.py
+ ${CMAKE_CURRENT_BINARY_DIR}/hpp/corbaserver/rbprm/common_idl.py
+)
+
+PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-rbprm)
+PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-model-urdf)
+PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-corbaserver)
+PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} omniORB4)
+PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-rbprm)
+
+INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION lib)
+
+# search for python
+FINDPYTHON()
+
+# Generate and install python code for each idl file
+FOREACH (IDL ${IDL_SOURCES})
+ ADD_CUSTOM_COMMAND(
+ OUTPUT
+ ${CMAKE_CURRENT_BINARY_DIR}/hpp/corbaserver/rbprm/${IDL}_idl.py
+ COMMAND ${OMNIIDL}
+ ARGS -bpython -Wbpackage="" -I${CMAKE_SOURCE_DIR}/idl
+ -I${HPP_CORBASERVER_DATAROOTDIR}/idl
+ ${CMAKE_SOURCE_DIR}/idl/hpp/corbaserver/rbprm/${IDL}.idl
+ MAIN_DEPENDENCY
+ ${CMAKE_SOURCE_DIR}/idl/hpp/corbaserver/rbprm/${IDL}.idl
+ )
+ SET_SOURCE_FILES_PROPERTIES(
+ ${CMAKE_CURRENT_BINARY_DIR}/${IDL}_idl.py
+ GENERATED=ON
+ )
+ INSTALL(
+ FILES
+ ${CMAKE_CURRENT_BINARY_DIR}/${IDL}_idl.py
+ DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm
+ )
+ENDFOREACH()
+
+# Generate and install python code for each dependency idl file
+FOREACH (IDL ${HPP_CORBASERVER_IDL_SOURCES})
+ ADD_CUSTOM_COMMAND(
+ OUTPUT
+ ${CMAKE_CURRENT_BINARY_DIR}/hpp/corbaserver/rbprm/${IDL}_idl.py
+ COMMAND ${OMNIIDL}
+ ARGS -bpython -Wbpackage=""
+ -I${HPP_CORBASERVER_DATAROOTDIR}/idl
+ ${HPP_CORBASERVER_DATAROOTDIR}/idl/hpp/corbaserver/${IDL}.idl
+ MAIN_DEPENDENCY
+ ${HPP_CORBASERVER_DATAROOTDIR}/idl/hpp/corbaserver/${IDL}.idl
+ )
+ SET_SOURCE_FILES_PROPERTIES(
+ ${CMAKE_CURRENT_BINARY_DIR}/${IDL}_idl.py
+ GENERATED=ON
+ )
+ INSTALL(
+ FILES
+ ${CMAKE_CURRENT_BINARY_DIR}/${IDL}_idl.py
+ DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm
+ )
+ENDFOREACH()
+
+INSTALL(
+ FILES
+ ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/__init__.py
+ DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm
+ )
+INSTALL(
+ FILES
+ ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/client.py
+ ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmbuilder.py
+ DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm
+ )
+
+# Stand alone corba server
+ADD_EXECUTABLE (hpp-rbprm-server hpp-rbprm-corba.cc)
+TARGET_LINK_LIBRARIES (hpp-rbprm-server ${LIBRARY_NAME} hpp-rbprm)
+PKG_CONFIG_USE_DEPENDENCY(hpp-rbprm-server hpp-model-urdf)
+
+INSTALL (TARGETS hpp-rbprm-server DESTINATION ${CMAKE_INSTALL_BINDIR})
diff --git a/src/fwd.hh b/src/fwd.hh
new file mode 100644
index 0000000000000000000000000000000000000000..854ee5b629015caff2dee4e267054c4fa2aac48b
--- /dev/null
+++ b/src/fwd.hh
@@ -0,0 +1,27 @@
+// Copyright (c) 2012 CNRS
+// Author: Florent Lamiraux
+//
+// This file is part of hpp-manipulation-corba.
+// hpp-manipulation-corba 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.
+//
+// hpp-manipulation-corba 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
+// hpp-manipulation-corba. If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_MANIPULATION_CORBA_FWD_HH
+# define HPP_MANIPULATION_CORBA_FWD_HH
+
+namespace hpp {
+ namespace manipulation {
+ namespace corba {
+ }
+ }
+}
+#endif // HPP_MANIPULATION_CORBA_FWD_HH
diff --git a/src/hpp-rbprm-corba.cc b/src/hpp-rbprm-corba.cc
new file mode 100644
index 0000000000000000000000000000000000000000..2c18cb05098f64ea5a347c6517b2edd433427b1f
--- /dev/null
+++ b/src/hpp-rbprm-corba.cc
@@ -0,0 +1,38 @@
+// Copyright (c) 2012 CNRS
+// Author: Florent Lamiraux
+//
+// This file is part of hpp-manipulation-corba.
+// hpp-manipulation-corba 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.
+//
+// hpp-manipulation-corba 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
+// hpp-manipulation-corba. If not, see
+// <http://www.gnu.org/licenses/>.
+
+#include <hpp/corbaserver/server.hh>
+#include <hpp/corbaserver/rbprm/server.hh>
+#include <hpp/core/problem-solver.hh>
+
+typedef hpp::rbprm::Server RbprmServer;
+typedef hpp::corbaServer::Server CorbaServer;
+
+int main (int argc, char* argv [])
+{
+ hpp::core::ProblemSolverPtr_t problemSolver (hpp::core::ProblemSolver::create());
+
+ CorbaServer corbaServer (problemSolver, argc,
+ const_cast<const char**> (argv), true);
+ RbprmServer rbprmServer (argc, argv, true, "rbprmChild");
+ rbprmServer.setProblemSolver (problemSolver);
+
+ corbaServer.startCorbaServer ();
+ rbprmServer.startCorbaServer ("hpp", "corbaserver",
+ "rbprm");
+ corbaServer.processRequest(true);
+}
diff --git a/src/hpp/corbaserver/rbprm/__init__.py b/src/hpp/corbaserver/rbprm/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..5191ae20a4eaabf4ef102cf57bff1d81ffb0d5f4
--- /dev/null
+++ b/src/hpp/corbaserver/rbprm/__init__.py
@@ -0,0 +1,7 @@
+import omniORB
+omniORB.updateModule("hpp.corbaserver.rbprm")
+
+import rbprmbuilder_idl
+
+from client import Client
+
diff --git a/src/hpp/corbaserver/rbprm/client.py b/src/hpp/corbaserver/rbprm/client.py
new file mode 100644
index 0000000000000000000000000000000000000000..eb3dd49d4fa17b8a06446a53f7627b89835c711d
--- /dev/null
+++ b/src/hpp/corbaserver/rbprm/client.py
@@ -0,0 +1,67 @@
+#!/usr/bin/env python
+#
+# Copyright (c) 2014 CNRS
+# Author: Steve Tonneau
+#
+# This file is part of hpp-rbprm-corba.
+# hpp-rbprm-corba 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.
+#
+# hpp-rbprm-corba 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
+# hpp-manipulation-corba. If not, see
+# <http://www.gnu.org/licenses/>.
+
+from omniORB import CORBA
+import CosNaming
+
+from hpp.corbaserver.rbprm import RbprmBuilder
+
+class CorbaError(Exception):
+ """
+ Raised when a CORBA error occurs.
+ """
+ def __init__(self, value):
+ self.value = value
+ def __str__(self):
+ return repr(self.value)
+
+class Client:
+ """
+ Connect and create clients for hpp-rbprm library.
+ """
+ def __init__(self):
+ """
+ Initialize CORBA and create default clients.
+ """
+ import sys
+ self.orb = CORBA.ORB_init (sys.argv, CORBA.ORB_ID)
+ obj = self.orb.resolve_initial_references("NameService")
+ self.rootContext = obj._narrow(CosNaming.NamingContext)
+ if self.rootContext is None:
+ raise CorbaError ('failed to narrow the root context')
+
+ # client of Rbprm interface
+ name = [CosNaming.NameComponent ("hpp", "corbaserver"),
+ CosNaming.NameComponent ("rbprm", "rbprmbuilder")]
+
+ try:
+ obj = self.rootContext.resolve (name)
+ except CosNaming.NamingContext.NotFound, ex:
+ raise CorbaError ('failed to find rbprm service.')
+ try:
+ client = obj._narrow (RbprmBuilder)
+ except KeyError:
+ raise CorbaError ('invalid service name rbprm')
+
+ if client is None:
+ # This happens when stubs from client and server are not synchronized.
+ raise CorbaError (
+ 'failed to narrow client for service rbprm')
+ self.rbprm = client
+
diff --git a/src/hpp/corbaserver/rbprm/rbprmbuilder.py b/src/hpp/corbaserver/rbprm/rbprmbuilder.py
new file mode 100644
index 0000000000000000000000000000000000000000..713cc3c3277ea3633f8a54b5d59d0dc0f2cb4850
--- /dev/null
+++ b/src/hpp/corbaserver/rbprm/rbprmbuilder.py
@@ -0,0 +1,241 @@
+#!/usr/bin/env python
+# Copyright (c) 2014 CNRS
+# Author: Steve Tonneau
+#
+# This file is part of hpp-rbprm-corba.
+# hpp-rbprm-corba 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.
+#
+# hpp-manipulation-corba 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
+# hpp-manipulation-corba. If not, see
+# <http://www.gnu.org/licenses/>.
+
+from hpp.corbaserver.rbprm import Client as RbprmClient
+from hpp.corbaserver import Client as BasicClient
+
+## Corba clients to the various servers
+#
+class CorbaClient:
+ """
+ Container for corba clients to various interfaces.
+ """
+ def __init__ (self):
+ self.basic = BasicClient ()
+ self.rbprm = RbprmClient ()
+
+## Load and handle a RbprmDevice robot for rbprm planning
+#
+# A RbprmDevice robot is a set of two robots. One for the
+# trunk of the robot, one for the range of motion
+class Builder (object):
+ ## Constructor
+ # \param trunkName name of the first robot that is loaded now,
+ # \param romName name of the first robot that is loaded now,
+ # \param rootJointType type of root joint among ("freeflyer", "planar",
+ # "anchor"),
+ # \param load whether to actually load urdf files. Set to no if you only
+ # want to initialize a corba client to an already initialized
+ # problem.
+ def __init__ (self, trunkName, romName, rootJointType, load = True):
+ self.tf_root = "base_link"
+ self.rootJointType = dict()
+ self.name = trunkName
+ self.displayName = trunkName
+ self.client = CorbaClient ()
+ self.load = load
+ self.loadModel (trunkName, romName, rootJointType)
+
+ ## Virtual function to load the robot model
+ def loadModel (self, trunkName, romName, rootJointType):
+ self.client.rbprm.robot.create (trunkName, romName)
+
+ ## \name Degrees of freedom
+ # \{
+
+ ## Get size of configuration
+ # \return size of configuration
+ def getConfigSize (self):
+ return self.client.basic.robot.getConfigSize ()
+
+ # Get size of velocity
+ # \return size of velocity
+ def getNumberDof (self):
+ return self.client.basic.robot.getNumberDof ()
+ ## \}
+
+ ## \name Joints
+ #\{
+
+ ## Get joint names in the same order as in the configuration.
+ def getJointNames (self):
+ return self.client.basic.robot.getJointNames ()
+
+ ## Get joint names in the same order as in the configuration.
+ def getAllJointNames (self):
+ return self.client.basic.robot.getAllJointNames ()
+
+ ## Get joint position.
+ def getJointPosition (self, jointName):
+ return self.client.basic.robot.getJointPosition (jointName)
+
+ ## Set static position of joint in its parent frame
+ def setJointPosition (self, jointName, position):
+ return self.client.basic.robot.setJointPosition (jointName, position)
+
+ ## Get joint number degrees of freedom.
+ def getJointNumberDof (self, jointName):
+ return self.client.basic.robot.getJointNumberDof (jointName)
+
+ ## Get joint number config size.
+ def getJointConfigSize (self, jointName):
+ return self.client.basic.robot.getJointConfigSize (jointName)
+
+ ## set bounds for the joint
+ def setJointBounds (self, jointName, inJointBound):
+ return self.client.basic.robot.setJointBounds (jointName, inJointBound)
+
+ ## Set bounds on the translation part of the freeflyer joint.
+ #
+ # Valid only if the robot has a freeflyer joint.
+ def setTranslationBounds (self, xmin, xmax, ymin, ymax, zmin, zmax):
+ self.client.basic.robot.setJointBounds \
+ (self.displayName + "base_joint_x", [xmin, xmax])
+ self.client.basic.robot.setJointBounds \
+ (self.displayName + "base_joint_y", [ymin, ymax])
+ self.client.basic.robot.setJointBounds \
+ (self.displayName + "base_joint_z", [zmin, zmax])
+
+ ## Get link position in joint frame
+ #
+ # Joints are oriented in a different way as in urdf standard since
+ # rotation and uni-dimensional translation joints act around or along
+ # their x-axis. This method returns the position of the urdf link in
+ # world frame.
+ #
+ # \param jointName name of the joint
+ # \return position of the link in world frame.
+ def getLinkPosition (self, jointName):
+ return self.client.basic.robot.getLinkPosition (jointName)
+
+ ## Get link name
+ #
+ # \param jointName name of the joint,
+ # \return name of the link.
+ def getLinkName (self, jointName):
+ return self.client.basic.robot.getLinkName (jointName)
+ ## \}
+
+ ## \name Access to current configuration
+ #\{
+
+ ## Set current configuration of composite robot
+ #
+ # \param q configuration of the composite robot
+ def setCurrentConfig (self, q):
+ self.client.basic.robot.setCurrentConfig (q)
+
+ ## Get current configuration of composite robot
+ #
+ # \return configuration of the composite robot
+ def getCurrentConfig (self):
+ return self.client.basic.robot.getCurrentConfig ()
+
+ ## Shoot random configuration
+ # \return dofArray Array of degrees of freedom.
+ def shootRandomConfig(self):
+ return self.client.basic.robot.shootRandomConfig ()
+
+ ## \}
+
+ ## \name Bodies
+ # \{
+
+ ## Get the list of objects attached to a joint.
+ # \param inJointName name of the joint.
+ # \return list of names of CollisionObject attached to the body.
+ def getJointInnerObjects (self, jointName):
+ return self.client.basic.robot.getJointInnerObjects (jointName)
+
+
+ ## Get list of collision objects tested with the body attached to a joint
+ # \param inJointName name of the joint.
+ # \return list of names of CollisionObject
+ def getJointOuterObjects (self, jointName):
+ return self.client.basic.robot.getJointOuterObjects (jointName)
+
+ ## Get position of robot object
+ # \param objectName name of the object.
+ # \return transformation as a hpp.Transform object
+ def getObjectPosition (self, objectName):
+ return Transform (self.client.basic.robot.getObjectPosition
+ (objectName))
+
+ ## \brief Remove an obstacle from outer objects of a joint body
+ #
+ # \param objectName name of the object to remove,
+ # \param jointName name of the joint owning the body,
+ # \param collision whether collision with object should be computed,
+ # \param distance whether distance to object should be computed.
+ def removeObstacleFromJoint (self, objectName, jointName, collision,
+ distance):
+ return self.client.basic.obstacle.removeObstacleFromJoint \
+ (objectName, jointName, collision, distance)
+
+
+ ## \}
+
+ ## \name Collision checking and distance computation
+ # \{
+
+ ## Test collision with obstacles and auto-collision.
+ #
+ # Check whether current configuration of robot is valid by calling
+ # CkwsDevice::collisionTest ().
+ # \return whether configuration is valid
+ # \note Deprecated. Use isConfigValid instead.
+ def collisionTest (self):
+ print "Deprecated. Use isConfigValid instead"
+ return self.client.basic.robot.collisionTest ()
+
+ ## Check the validity of a configuration.
+ #
+ # Check whether a configuration of robot is valid.
+ # \param cfg a configuration
+ # \return whether configuration is valid
+ def isConfigValid (self, cfg):
+ return self.client.basic.robot.isConfigValid (cfg)
+
+ ## Compute distances between bodies and obstacles
+ #
+ # \return list of distances,
+ # \return names of the objects belonging to a body
+ # \return names of the objects tested with inner objects,
+ # \return closest points on the body,
+ # \return closest points on the obstacles
+ # \note outer objects for a body can also be inner objects of another
+ # body.
+ def distancesToCollision (self):
+ return self.client.basic.robot.distancesToCollision ()
+ ## \}
+
+ ## \}
+ ## \name Mass and inertia
+ # \{
+
+ ## Get mass of robot
+ def getMass (self):
+ return self.client.basic.robot.getMass ()
+
+ ## Get position of center of mass
+ def getCenterOfMass (self):
+ return self.client.basic.robot.getCenterOfMass ()
+ ## Get Jacobian of the center of mass
+ def getJacobianCenterOfMass (self):
+ return self.client.basic.robot.getJacobianCenterOfMass ()
+ ##\}
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
new file mode 100644
index 0000000000000000000000000000000000000000..de784c01c0bb7e3e35acc99667167f9cb985744b
--- /dev/null
+++ b/src/rbprmbuilder.impl.cc
@@ -0,0 +1,90 @@
+// Copyright (c) 2012 CNRS
+// Author: Florent Lamiraux, Joseph Mirabel
+//
+// This file is part of hpp-manipulation-corba.
+// hpp-manipulation-corba 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.
+//
+// hpp-manipulation-corba 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
+// hpp-manipulation-corba. If not, see
+// <http://www.gnu.org/licenses/>.
+
+//#include <hpp/fcl/math/transform.h>
+#include <hpp/util/debug.hh>
+#include "rbprmbuilder.impl.hh"
+#include "hpp/rbprm/rbprm-device.hh"
+#include "hpp/model/urdf/util.hh"
+#include <hpp/core/problem-solver.hh>
+
+namespace hpp {
+ namespace rbprm {
+ namespace impl {
+
+ void RbprmBuilder::loadRobotRomModel(const char* robotName,
+ const char* rootJointType,
+ const char* packageName,
+ const char* modelName,
+ const char* urdfSuffix,
+ const char* srdfSuffix) throw (hpp::Error)
+ {
+ try
+ {
+ romDevice_ = model::Device::create (robotName);
+ hpp::model::urdf::loadRobotModel (romDevice_,
+ std::string (rootJointType),
+ std::string (packageName),
+ std::string (modelName),
+ std::string (urdfSuffix),
+ std::string (srdfSuffix));
+ }
+ catch (const std::exception& exc)
+ {
+ hppDout (error, exc.what ());
+ throw hpp::Error (exc.what ());
+ }
+ romLoaded_ = true;
+ }
+
+ void RbprmBuilder::loadRobotCompleteModel(const char* robotName,
+ const char* rootJointType,
+ const char* packageName,
+ const char* modelName,
+ const char* urdfSuffix,
+ const char* srdfSuffix) throw (hpp::Error)
+ {
+ if(!romLoaded_)
+ {
+ std::string err("Rom must be loaded before loading complete model") ;
+ hppDout (error, err ());
+ throw hpp::Error(err.c_str());
+ }
+ try
+ {
+ hpp::model::RbPrmDevicePtr_t device = hpp::model::RbPrmDevice::create (robotName, romDevice_);
+ hpp::model::urdf::loadRobotModel (device,
+ std::string (rootJointType),
+ std::string (packageName),
+ std::string (modelName),
+ std::string (urdfSuffix),
+ std::string (srdfSuffix));
+ // Add device to the planner
+ problemSolver_->robot (device);
+ problemSolver_->robot ()->controlComputation
+ (model::Device::JOINT_POSITION);
+ }
+ catch (const std::exception& exc)
+ {
+ hppDout (error, exc.what ());
+ throw hpp::Error (exc.what ());
+ }
+ }
+
+ } // namespace impl
+ } // namespace rbprm
+} // namespace hpp
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
new file mode 100644
index 0000000000000000000000000000000000000000..e3695c57f92b128f54c04b148ebbde006763a614
--- /dev/null
+++ b/src/rbprmbuilder.impl.hh
@@ -0,0 +1,64 @@
+// Copyright (c) 2014 CNRS
+// Author: Florent Lamiraux
+//
+// This file is part of hpp-manipulation-corba.
+// hpp-manipulation-corba 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.
+//
+// hpp-manipulation-corba 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
+// hpp-manipulation-corba. If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_RBPRM_CORBA_BUILDER_IMPL_HH
+# define HPP_RBPRM_CORBA_BUILDER_IMPL_HH
+
+# include <hpp/corbaserver/rbprm/fwd.hh>
+# include <hpp/core/problem-solver.hh>
+# include "rbprmbuilder.hh"
+#include "hpp/rbprm/rbprm-device.hh"
+
+namespace hpp {
+ namespace rbprm {
+ namespace impl {
+ using CORBA::Short;
+
+ class RbprmBuilder : public virtual POA_hpp::corbaserver::rbprm::RbprmBuilder
+ {
+ public:
+ RbprmBuilder ()
+ : POA_hpp::corbaserver::rbprm::RbprmBuilder()
+ , romLoaded_(false) {}
+
+ virtual void loadRobotRomModel (const char* robotName,
+ const char* rootJointType,
+ const char* packageName,
+ const char* modelName,
+ const char* urdfSuffix,
+ const char* srdfSuffix) throw (hpp::Error);
+
+ virtual void loadRobotCompleteModel (const char* robotName,
+ const char* rootJointType,
+ const char* packageName,
+ const char* modelName,
+ const char* urdfSuffix,
+ const char* srdfSuffix) throw (hpp::Error);
+
+ public:
+ /// \brief Pointer to hppPlanner object of hpp::corbaServer::Server.
+ core::ProblemSolverPtr_t problemSolver_;
+
+ private:
+ model::DevicePtr_t romDevice_;
+ bool romLoaded_;
+ }; // class RobotBuilder
+ } // namespace impl
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_RBPRM_CORBA_BUILDER_IMPL_HH
diff --git a/src/server.cc b/src/server.cc
new file mode 100644
index 0000000000000000000000000000000000000000..7537767dcbfbd9c56a79ffe902cd618931399fda
--- /dev/null
+++ b/src/server.cc
@@ -0,0 +1,53 @@
+// Copyright (C) 2014 CNRS-LAAS
+// Author: Florent Lamiraux.
+//
+// This file is part of the hpp-manipulation-corba.
+//
+// hpp-manipulation-corba 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.
+//
+// hpp-manipulation-corba 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License along with hpp-manipulation-corba. If not, see
+// <http://www.gnu.org/licenses/>.
+
+#include <hpp/util/exception.hh>
+#include "rbprmbuilder.impl.hh"
+#include <hpp/corbaserver/rbprm/server.hh>
+
+namespace hpp {
+ namespace rbprm {
+ Server::Server (int argc, char *argv[], bool multiThread,
+ const std::string& poaName) :
+ rbprmBuilder_ (new corba::Server <impl::RbprmBuilder>
+ (argc, argv, multiThread, poaName)) {}
+
+ Server::~Server ()
+ {
+ delete rbprmBuilder_;
+ }
+
+ void Server::setProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver)
+ {
+ rbprmBuilder_->implementation ().problemSolver_ = problemSolver;
+ }
+
+ /// Start corba server
+ void Server::startCorbaServer(const std::string& contextId,
+ const std::string& contextKind,
+ const std::string& objectId)
+ {
+ if (rbprmBuilder_->startCorbaServer(contextId, contextKind,
+ objectId, "rbprmbuilder") != 0) {
+ HPP_THROW_EXCEPTION (hpp::Exception,
+ "Failed to start corba rbprm server.");
+ }
+ }
+ } // namespace rbprm
+} // namespace hpp