Commit 10221395 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

integration of fullbody

parent 8688802b
......@@ -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
......
......@@ -11,8 +11,8 @@ srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [0, 2, -2, 0, 0, 1.5])
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
......@@ -23,36 +23,11 @@ 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
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)
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)
......@@ -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
)
......
......@@ -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
# \{
......
#!/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 ()
##\}
......@@ -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)
......
......@@ -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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment