From 6ced4a80908185bbdf454505d1c5696399d7a9ef Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Thu, 14 Sep 2017 13:58:02 +0200
Subject: [PATCH] Add 'addNonContactingLimb' in the API : add a limb that we
can move to avoid collision but do not try to make contact with
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 7 ++++++
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 12 ++++++++++
src/rbprmbuilder.impl.cc | 26 +++++++++++++++-------
src/rbprmbuilder.impl.hh | 2 ++
4 files changed, 39 insertions(+), 8 deletions(-)
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index afecaf2..566923b 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -254,6 +254,13 @@ module hpp
in double resolution, in string contactType, in double disableEffectorCollision,
in double grasp,in floatSeq limbOffset) raises (Error);
+ /// Add a limb not used for contact generation
+ /// \param id user given name of the new limb
+ /// \param limb robot joint corresponding to the root of the limb (ex a shoulder or ankle joint)
+ /// \param effector robot joint corresponding to the effector of the limb (ex a hand or foot joint)
+ /// \param samples number of samples to generate for the limb (a typical value is 10000)
+ void addNonContactingLimb(in string id, in string limb, in string effector, in unsigned long samples) raises (Error);
+
/// Specifies a subchain of the robot as a limb, which can be used to create contacts.
/// A limb must consist in a simple kinematic chain, where every node has only one child
/// \param databasepath filepath to the database
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 979b07a..1690b13 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -124,6 +124,18 @@ class FullBody (object):
self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, "EFORT", 0.03)
self.limbNames += [limbId]
+ ## Add a limb no used for contact generation to the model
+ #
+ # \param id: user defined id for the limb. Must be unique.
+ # The id is used if several contact points are defined for the same limb (ex: the knee and the foot)
+ # \param name: name of the joint corresponding to the root of the limb.
+ # \param effectorName name of the joint to be considered as the effector of the limb
+ # \param collisionObjects objects to be considered for collisions with the limb. TODO remove
+ # \param nbSamples number of samples to generate for the limb
+ def addNonContactingLimb(self, limbId, name, effectorname, samples):
+ self.client.rbprm.rbprm.addNonContactingLimb(limbId, name, effectorname, samples)
+ self.limbNames += [limbId]
+
## Add a limb to the model
#
# \param databasepath: path to the database describing the robot
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 34f6019..efc5df0 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -1096,6 +1096,21 @@ namespace hpp {
}
}
+ void RbprmBuilder::addNonContactingLimb(const char* id, const char* limb, const char* effector, unsigned int samples) throw (hpp::Error)
+ {
+ if(!fullBodyLoaded_)
+ throw Error ("No full body robot was loaded");
+ try
+ {
+
+ fullBody()->AddNonContactingLimb(std::string(id), std::string(limb), std::string(effector), problemSolver()->collisionObstacles(), samples);
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
void RbprmBuilder::addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues, double disableEffectorCollision, double grasp) throw (hpp::Error)
{
@@ -2638,19 +2653,14 @@ assert(s2 == s1 +1);
rbprm::sampling::ValueBound bounds;
if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded");
- T_Limb::const_iterator lit = fullBody()->GetLimbs().find(std::string(limbname));
- if(lit == fullBody()->GetLimbs().end())
- {
- std::string err("No limb " + std::string(limbname) + "was defined for robot" + fullBody()->device_->name());
- throw Error (err.c_str());
- }
+ rbprm::RbPrmLimbPtr_t limb = fullBody()->GetLimb(limbname);
std::string eval(analysis);
if (eval == "all")
{
for(sampling::T_evaluate::const_iterator analysisit = analysisFactory_->evaluate_.begin();
analysisit != analysisFactory_->evaluate_.end(); ++ analysisit)
{
- sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (lit->second->sampleContainer_);
+ sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (limb->sampleContainer_);
sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
bounds = sampleDB.valueBounds_[analysisit->first];
}
@@ -2663,7 +2673,7 @@ assert(s2 == s1 +1);
std::string err("No analysis named " + eval + "was defined for analyzing database sample");
throw Error (err.c_str());
}
- sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (lit->second->sampleContainer_);
+ sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (limb->sampleContainer_);
sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
bounds = sampleDB.valueBounds_[analysisit->first];
}
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 78ba115..f886eb4 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -241,6 +241,8 @@ namespace hpp {
virtual void addLimb(const char* id, const char* limb, const char* effector, const hpp::floatSeq& offset, const hpp::floatSeq& normal, double x, double y,
unsigned int samples, const char *heuristicName, double resolution, const char *contactType,
double disableEffectorCollision, double grasp,const hpp::floatSeq& limbOffset) throw (hpp::Error);
+ virtual void addNonContactingLimb(const char* id, const char* limb, const char* effector, unsigned int samples) throw (hpp::Error);
+
virtual void addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues,
double disableEffectorCollision, double grasp) throw (hpp::Error);
--
GitLab