Skip to content
Snippets Groups Projects
Commit 428beda7 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

first commit

parent cb24999d
No related branches found
No related tags found
No related merge requests found
Showing with 1050 additions and 0 deletions
# 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()
# 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
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
// 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
// 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.
// 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
// 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
# 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})
// 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
// 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);
}
import omniORB
omniORB.updateModule("hpp.corbaserver.rbprm")
import rbprmbuilder_idl
from client import Client
#!/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
#!/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 ()
##\}
// 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
// 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
// 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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment