diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 23813b02a63d703fb4839391a2b313c412fe685c..7abb2138cf402134d876557d59962129b09db9d2 100644
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -67,6 +67,32 @@ module hpp
in string packageName, in string modelName,
in string urdfSuffix, in string srdfSuffix)
raises (Error);
+
+ /// Create a RbprmFullBody object
+ /// 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 loadFullBodyRobot (in string trunkRobotName, in string rootJointType,
+ in string packageName, in string modelName,
+ in string urdfSuffix, in string srdfSuffix)
+ raises (Error);
+
+ /// Get Sample configuration
+ /// \return dofArray Array of degrees of freedom.
+ floatSeq getSampleConfig(in string sampleName, in unsigned short sampleId) raises (Error);
+
+ void addLimb(in string limb, in unsigned short samples, in double resolution) raises (Error);
+
}; // interface Robot
}; // module rbprm
}; // module corbaserver
diff --git a/script/load2.py b/script/load2.py
deleted file mode 100644
index 4fb7bf8d1078c7c69373c00a5f35e8668ff99c65..0000000000000000000000000000000000000000
--- a/script/load2.py
+++ /dev/null
@@ -1,58 +0,0 @@
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
-from hpp.gepetto import Viewer
-
-rootJointType = 'freeflyer'
-packageName = 'hpp-rbprm-corba'
-meshPackageName = 'hpp-rbprm-corba'
-urdfName = 'box'
-urdfNameRom = 'box_rom'
-urdfSuffix = ""
-srdfSuffix = ""
-
-rbprmBuilder = Builder ()
-
-rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [0, 2, -2, 0, 0, 1.5])
-
-#~ from hpp.corbaserver.rbprm. import ProblemSolver
-from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
-
-ps = ProblemSolver( rbprmBuilder )
-
-r = Viewer (ps)
-
-q_init = rbprmBuilder.getCurrentConfig ()
-q_goal = q_init [::]
-q_init [0:3] = [0, -0.5, 0.4]
-
-#~ rank = rbprmBuilder.rankInConfiguration ['torso_lift_joint']
-#~ q_init [rank] = 0.2
-r (q_init)
-
-q_goal [0:3] = [1, -0.5, 1]
-#~ q_goal [0:3] = [-3.2, 0, 3]
-#~ rank = rbprmBuilder.rankInConfiguration ['l_shoulder_lift_joint']
-#~ q_goal [rank] = 0.5
-#~ rank = rbprmBuilder.rankInConfiguration ['l_elbow_flex_joint']
-#~ q_goal [rank] = -0.5
-#~ rank = rbprmBuilder.rankInConfiguration ['r_shoulder_lift_joint']
-#~ q_goal [rank] = 0.5
-#~ rank = rbprmBuilder.rankInConfiguration ['r_elbow_flex_joint']
-#~ q_goal [rank] = -0.5
-r (q_goal)
-
-r.loadObstacleModel (packageName, "scene", "car")
-
-ps.setInitialConfig (q_init)
-ps.addGoalConfig (q_goal)
-
-ps.client.problem.selectConFigurationShooter("RbprmShooter")
-
-ps.solve ()
-
-
-from hpp.gepetto import PathPlayer
-pp = PathPlayer (rbprmBuilder.client.basic, r)
-
-pp (0)
-
diff --git a/script/loadfullbody.py b/script/loadfullbody.py
new file mode 100644
index 0000000000000000000000000000000000000000..2df2ed8a81c5dbfe20006b63eb3058b45703dcdb
--- /dev/null
+++ b/script/loadfullbody.py
@@ -0,0 +1,33 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'box'
+urdfNameRom = 'box_rom'
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+
+rbprmBuilder.loadFullBodyModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [0, 2, -2, 0, -1, 1.5])
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( rbprmBuilder )
+
+r = Viewer (ps)
+
+q_init = rbprmBuilder.getCurrentConfig ()
+q_goal = q_init [::]
+q_init [0:3] = [0, -0.5, 0.3]
+q_goal = [0, -0.5, -0.2, -0.501544,0.431183, 0.662926, -0.350804]
+r (q_goal)
+
+
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 38f58e75f3df3f77dfaa92196b8a3e681568316c..9dfcbbbe5bc5c125f8f9c937c9d7688f8a1eba27 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -119,6 +119,7 @@ INSTALL(
FILES
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/client.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmbuilder.py
+ ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmfullbody.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/problem_solver.py
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm
)
diff --git a/src/hpp/corbaserver/rbprm/rbprmbuilder.py b/src/hpp/corbaserver/rbprm/rbprmbuilder.py
index 9a47066e71f9d20665b2cc706b95aa63a4e7409a..0370e8e753ae0c8300e1225a2bfc083217b6d312 100644
--- a/src/hpp/corbaserver/rbprm/rbprmbuilder.py
+++ b/src/hpp/corbaserver/rbprm/rbprmbuilder.py
@@ -72,6 +72,29 @@ class Builder (object):
rankInConfiguration += self.client.basic.robot.getJointConfigSize (j)
self.rankInVelocity [j] = rankInVelocity
rankInVelocity += self.client.basic.robot.getJointNumberDof (j)
+
+ def loadFullBodyModel (self, urdfName, rootJointType, packageName, meshPackageName, urdfSuffix, srdfSuffix):
+ self.client.rbprm.rbprm.loadloadFullBodyRobot(urdfName, rootJointType, packageName, urdfNamerom, urdfSuffix, srdfSuffix)
+ self.name = urdfName
+ self.displayName = urdfName
+ self.tf_root = "base_link"
+ self.rootJointType = rootJointType
+ self.jointNames = self.client.basic.robot.getJointNames ()
+ self.allJointNames = self.client.basic.robot.getAllJointNames ()
+ self.client.basic.robot.meshPackageName = meshPackageName
+ self.meshPackageName = meshPackageName
+ self.rankInConfiguration = dict ()
+ self.rankInVelocity = dict ()
+ self.packageName = packageName
+ self.urdfName = urdfName
+ self.urdfSuffix = urdfSuffix
+ self.srdfSuffix = srdfSuffix
+ rankInConfiguration = rankInVelocity = 0
+ for j in self.jointNames:
+ self.rankInConfiguration [j] = rankInConfiguration
+ rankInConfiguration += self.client.basic.robot.getJointConfigSize (j)
+ self.rankInVelocity [j] = rankInVelocity
+ rankInVelocity += self.client.basic.robot.getJointNumberDof (j)
## Init RbprmShooter
#
@@ -81,6 +104,8 @@ class Builder (object):
return self.client.rbprm.rbprm.initshooter ()
## \}
+
+
## \name Degrees of freedom
# \{
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
new file mode 100644
index 0000000000000000000000000000000000000000..3c2a821c905e8b557faaa4d59e2901005af3d30a
--- /dev/null
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -0,0 +1,257 @@
+#!/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, load = True):
+ self.tf_root = "base_link"
+ self.rootJointType = dict()
+ self.client = CorbaClient ()
+ self.load = load
+
+ def loadFullBodyModel (self, urdfName, rootJointType, packageName, meshPackageName, urdfSuffix, srdfSuffix):
+ self.client.rbprm.rbprm.loadloadFullBodyRobot(urdfName, rootJointType, packageName, urdfNamerom, urdfSuffix, srdfSuffix)
+ self.name = urdfName
+ self.displayName = urdfName
+ self.tf_root = "base_link"
+ self.rootJointType = rootJointType
+ self.jointNames = self.client.basic.robot.getJointNames ()
+ self.allJointNames = self.client.basic.robot.getAllJointNames ()
+ self.client.basic.robot.meshPackageName = meshPackageName
+ self.meshPackageName = meshPackageName
+ self.rankInConfiguration = dict ()
+ self.rankInVelocity = dict ()
+ self.packageName = packageName
+ self.urdfName = urdfName
+ self.urdfSuffix = urdfSuffix
+ self.srdfSuffix = srdfSuffix
+ rankInConfiguration = rankInVelocity = 0
+ for j in self.jointNames:
+ self.rankInConfiguration [j] = rankInConfiguration
+ rankInConfiguration += self.client.basic.robot.getJointConfigSize (j)
+ self.rankInVelocity [j] = rankInVelocity
+ rankInVelocity += self.client.basic.robot.getJointNumberDof (j)
+
+ ## \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
index 734e16c4a61681e88bcd50e3cf8f8ad71ed45e1d..d6c1bebe16614cba76013f4f462294ce7a92c16c 100644
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -34,6 +34,7 @@ namespace hpp {
RbprmBuilder::RbprmBuilder ()
: POA_hpp::corbaserver::rbprm::RbprmBuilder()
, romLoaded_(false)
+ , fullBodyLoaded_(false)
, bindShooter_()
{
// NOTHING
@@ -98,6 +99,76 @@ namespace hpp {
}
}
+ void RbprmBuilder::loadFullBodyRobot(const char* robotName,
+ const char* rootJointType,
+ const char* packageName,
+ const char* modelName,
+ const char* urdfSuffix,
+ const char* srdfSuffix) throw (hpp::Error)
+ {
+ try
+ {
+ model::DevicePtr_t device = model::Device::create (robotName);
+ hpp::model::urdf::loadRobotModel (romDevice_,
+ std::string (rootJointType),
+ std::string (packageName),
+ std::string (modelName),
+ std::string (urdfSuffix),
+ std::string (srdfSuffix));
+ fullBody_ = rbprm::RbPrmFullBody::create(device);
+ }
+ catch (const std::exception& exc)
+ {
+ hppDout (error, exc.what ());
+ throw hpp::Error (exc.what ());
+ }
+ fullBodyLoaded_ = true;
+ }
+
+
+ hpp::floatSeq* RbprmBuilder::getSampleConfig(const char* limb, unsigned short sampleId) throw (hpp::Error)
+ {
+ if(!fullBodyLoaded_)
+ throw Error ("No full body robot was loaded");
+ const T_Limb& limbs = fullBody_->GetLimbs();
+ T_Limb::const_iterator lit = limbs.find(std::string(limb));
+ if(lit == limbs.end())
+ {
+ std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody_->device_->name());
+ throw Error (err.c_str());
+ }
+ const RbPrmLimbPtr_t& limbPtr = lit->second;
+ hpp::floatSeq *dofArray;
+ Eigen::VectorXd config = fullBody_->device_->currentConfiguration ();
+ if(sampleId > limbPtr->sampleContainer_.samples_.size())
+ {
+ std::string err("Limb " + std::string(limb) + "does not have samples.");
+ throw Error (err.c_str());
+ }
+ const sampling::Sample& sample = limbPtr->sampleContainer_.samples_[sampleId];
+ size_t ric = sample.startRank_;
+ config.tail(ric) = sample.configuration_;
+ dofArray = new hpp::floatSeq();
+ dofArray->length(_CORBA_ULong(config.rows()));
+ for(std::size_t i=0; i< _CORBA_ULong(config.rows()); i++)
+ (*dofArray)[(_CORBA_ULong)i] = config [i];
+ return dofArray;
+ }
+
+ void RbprmBuilder::addLimb(const char* limb, unsigned short samples, double resolution) throw (hpp::Error)
+ {
+ if(!fullBodyLoaded_)
+ throw Error ("No full body robot was loaded");
+ try
+ {
+ fullBody_->AddLimb(std::string(limb),samples,resolution);
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
namespace
{
hpp::core::PathValidationPtr_t createPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val)
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index eedf7d1015ff34d453e58c4ac430312e3b3523f4..3b324399df953e63da44eab818d0c1153e083cc3 100644
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -21,6 +21,7 @@
# include <hpp/core/problem-solver.hh>
# include "rbprmbuilder.hh"
# include <hpp/rbprm/rbprm-device.hh>
+# include <hpp/rbprm/rbprm-fullbody.hh>
# include <hpp/rbprm/rbprm-shooter.hh>
# include <hpp/rbprm/rbprm-validation.hh>
@@ -66,6 +67,18 @@ namespace hpp {
const char* urdfSuffix,
const char* srdfSuffix) throw (hpp::Error);
+
+ virtual void loadFullBodyRobot (const char* robotName,
+ const char* rootJointType,
+ const char* packageName,
+ const char* modelName,
+ const char* urdfSuffix,
+ const char* srdfSuffix) throw (hpp::Error);
+
+ virtual hpp::floatSeq* getSampleConfig(const char* limb, unsigned short sampleId) throw (hpp::Error);
+
+ virtual void addLimb(const char* limb, unsigned short samples, double resolution) throw (hpp::Error);
+
public:
void SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver);
@@ -75,7 +88,9 @@ namespace hpp {
private:
model::DevicePtr_t romDevice_;
+ rbprm::RbPrmFullBodyPtr_t fullBody_;
bool romLoaded_;
+ bool fullBodyLoaded_;
BindShooter bindShooter_;
}; // class RobotBuilder
} // namespace impl