first commit

CMakeLists.txt

+# 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()
+

build/src/hpp/corbaserver/rbprm/__init__.py

+# 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

doc/Doxyfile.extra.in

+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

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

+// 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

include/hpp/corbaserver/rbprm/doc.hh.in

+// 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.

include/hpp/corbaserver/rbprm/fwd.hh

+// 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

include/hpp/corbaserver/rbprm/server.hh

+// 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

src/CMakeLists.txt

+# 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})

src/fwd.hh

+// 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

src/hpp-rbprm-corba.cc

+// 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);
+}

src/hpp/corbaserver/rbprm/__init__.py

+import omniORB
+omniORB.updateModule("hpp.corbaserver.rbprm")
+
+import rbprmbuilder_idl
+
+from client import Client
+

src/hpp/corbaserver/rbprm/client.py

+#!/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
+

src/hpp/corbaserver/rbprm/rbprmbuilder.py

+#!/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 ()
+    ##\}

src/rbprmbuilder.impl.cc

+// 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

src/rbprmbuilder.impl.hh

+// 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

src/server.cc

+// 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