diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 83810e737c74ed5ae6d8ffdb295f7270c5efdca1..fd201e87ac28c77bb42f8f23850f793ed474ab9c 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -178,9 +178,10 @@ module hpp
/// \param heuristicName heuristic used to bias sample selection
/// \param resolution resolution of the octree used to store the samples (a typical value is 0.01 meters)
/// \param contactType whether the contact is punctual ("_3_DOF") or surfacic ("_6_DOF")
+ /// \param disableEffectorCollision whether collision detection should be disabled for the end effector bones
void addLimb(in string id, in string limb, in string effector, in floatSeq offset, in floatSeq normal,
in double x, in double y, in unsigned short samples, in string heuristicName,
- in double resolution, in string contactType) raises (Error);
+ in double resolution, in string contactType, in double disableEffectorCollision) 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
@@ -188,7 +189,9 @@ module hpp
/// \param id user given name of the new limb
/// \param heuristicName heuristic used to bias sample selection
/// \param loadValues whether other values computed for the limb database should be loaded
- void addLimbDatabase(in string databasepath, in string id, in string heuristicName, in double loadValues) raises (Error);
+ /// \param disableEffectorCollision whether collision detection should be disabled for the end effector bones
+ void addLimbDatabase(in string databasepath, in string id, in string heuristicName, in double loadValues,
+ in double disableEffectorCollision) raises (Error);
/// Set the start state of a contact generation problem
/// environment, ordered by their efficiency
diff --git a/script/scenarios/ground_crouch_hyq_interp.py b/script/scenarios/ground_crouch_hyq_interp.py
index 2e60bca6ec4c5e8da3f565a18d5718e8b799e6cc..4ba8f75b15e81dc910369876a040682fd7ce8f9d 100755
--- a/script/scenarios/ground_crouch_hyq_interp.py
+++ b/script/scenarios/ground_crouch_hyq_interp.py
@@ -1,5 +1,6 @@
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
import ground_crouch_hyq_path as tp
@@ -26,45 +27,43 @@ r = tp.Viewer (ps)
rootName = 'base_joint_xyz'
-#~ AFTER loading obstacles
+# Creating limbs
+# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
cType = "_3_DOF"
+# string identifying the limb
rLegId = 'rfleg'
+# First joint of the limb, as in urdf file
rLeg = 'rf_haa_joint'
+# Last joint of the limb, as in urdf file
rfoot = 'rf_foot_joint'
-rLegOffset = [0.,0,0.]
-rLegNormal = [0,1,0]
-rLegx = 0.02; rLegy = 0.02
-fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "forward", 0.1,cType)
+# Specifying the distance between last joint and contact surface
+offset = [0.,-0.021,0.]
+# Specifying the contact surface direction when the limb is in rest pose
+normal = [0,1,0]
+# Specifying the rectangular contact surface length
+legx = 0.02; legy = 0.02
+# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
+fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
lfoot = 'lh_foot_joint'
-lLegOffset = [0,0,0]
-lLegNormal = [0,1,0]
-lLegx = 0.02; lLegy = 0.02
-fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "backward", 0.05,cType)
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
-rArmOffset = [0.,0,-0.]
-rArmNormal = [0,1,0]
-rArmx = 0.02; rArmy = 0.02
-fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "backward", 0.05,cType)
+fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
-lArmOffset = [0.,0,-0.]
-lArmNormal = [0,1,0]
-lArmx = 0.02; lArmy = 0.02
-fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "forward", 0.05,cType)
+fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
-
fullBody.setCurrentConfig (q_init)
q_init = fullBody.generateContacts(q_init, [0,0,1])
q_0 = fullBody.getCurrentConfig();
@@ -84,9 +83,9 @@ r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
i = 0;
r (configs[i]); i=i+1; i-1
-q0 = configs[2]
-q0 = fullBody.generateContacts(q0, [0,0,1])
-r(q0)
+#~ q0 = configs[2]
+#~ q0 = fullBody.generateContacts(q0, [0,0,1])
+#~ r(q0)
c = fullBody.getContactSamplesIds("rfleg",q_init, [0,0,1])
-r(fullBody.getSample("rfleg",int(c[i]))); i = i+1
+#~ r(fullBody.getSample("rfleg",int(c[i]))); i = i+1
diff --git a/script/scenarios/ground_crouch_hyq_path.py b/script/scenarios/ground_crouch_hyq_path.py
index b5f1f81b71df4cae783e1bcf9bfa6dd51fc04c08..c3d48f92deab21c648f35ca887a7260f85d121d3 100755
--- a/script/scenarios/ground_crouch_hyq_path.py
+++ b/script/scenarios/ground_crouch_hyq_path.py
@@ -30,9 +30,7 @@ r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-5, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-
q_goal = q_init [::]
-
q_goal [0:3] = [5, 0, 0.63]; r (q_goal)
ps.addPathOptimizer("RandomShortcut")
@@ -44,6 +42,8 @@ ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
r.loadObstacleModel (packageName, "groundcrouch", "planning")
#~ ps.solve ()
t = ps.solve ()
+
+
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
f = open('log.txt', 'a')
diff --git a/script/scenarios/stair_bauzil_hrp2_interp.py b/script/scenarios/stair_bauzil_hrp2_interp.py
index 0dc48b34bf87165bd8f733397a6f784a820665dd..2f80a41b2fbea38782e6729fe6dc26858c1dad46 100755
--- a/script/scenarios/stair_bauzil_hrp2_interp.py
+++ b/script/scenarios/stair_bauzil_hrp2_interp.py
@@ -45,7 +45,8 @@ rHand = 'RARM_JOINT5'
rArmOffset = [0,0,-0.1]
rArmNormal = [0,0,1]
rArmx = 0.024; rArmy = 0.024
-fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.05)
+#disabling collision for hook
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
#~ AFTER loading obstacles
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 77479a1b323b4b20afea60ab80e1970af4c4a0e6..2686320aa01ad14f22635334625624b54840e0fa 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -101,11 +101,15 @@ class FullBody (object):
# The id is used if several contact points are defined for the same limb (ex: the knee and the foot)
# \param heuristicName: name of the selected heuristic for configuration evaluation
# \param loadValues: whether values computed, other than the static ones, should be loaded in memory
- def addLimbDatabase(self, databasepath, limbId, heuristicName, loadValues = True):
+ # \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
+ def addLimbDatabase(self, databasepath, limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
boolVal = 0.
+ boolValEff = 0.
if(loadValues):
boolVal = 1.
- self.client.rbprm.rbprm.addLimbDatabase(databasepath, limbId, heuristicName, boolVal)
+ if(disableEffectorCollision):
+ boolValEff = 1.
+ self.client.rbprm.rbprm.addLimbDatabase(databasepath, limbId, heuristicName, boolVal,boolValEff)
## Add a limb to the model
#
@@ -124,8 +128,12 @@ class FullBody (object):
# of the unit voxel of the octree. The larger they are, the more samples will be considered as candidates for contact.
# This can be problematic in terms of performance. The default value is 3 cm.
# \param contactType whether the contact is punctual ("_3_DOF") or surfacic ("_6_DOF")
- def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution, contactType="_6_DOF"):
- self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution,contactType)
+ # \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
+ def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution, contactType="_6_DOF",disableEffectorCollision = False):
+ boolValEff = 0.
+ if(disableEffectorCollision):
+ boolValEff = 1.
+ self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution,contactType, boolValEff)
## Returns the configuration of a limb described by a sample
#
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 39bd42f21f216a36ed0a2096ff311dd881a6669d..a3840b9826a992253c7cd4f28e47f979bd2eb6bd 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -451,7 +451,7 @@ namespace hpp {
}
void RbprmBuilder::addLimb(const char* id, const char* limb, const char* effector, const hpp::floatSeq& offset, const hpp::floatSeq& normal, double x, double y,
- unsigned short samples, const char* heuristicName, double resolution, const char *contactType) throw (hpp::Error)
+ unsigned short samples, const char* heuristicName, double resolution, const char *contactType, double disableEffectorCollision) throw (hpp::Error)
{
if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded");
@@ -468,7 +468,8 @@ namespace hpp {
{
cType = hpp::rbprm::_3_DOF;
}
- fullBody_->AddLimb(std::string(id), std::string(limb), std::string(effector), off, norm, x, y, problemSolver_->collisionObstacles(), samples,heuristicName,resolution,cType);
+ fullBody_->AddLimb(std::string(id), std::string(limb), std::string(effector), off, norm, x, y,
+ problemSolver_->collisionObstacles(), samples,heuristicName,resolution,cType,disableEffectorCollision > 0.5);
}
catch(std::runtime_error& e)
{
@@ -477,14 +478,15 @@ namespace hpp {
}
- void RbprmBuilder::addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues) throw (hpp::Error)
+ void RbprmBuilder::addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues, double disableEffectorCollision) throw (hpp::Error)
{
if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded");
try
{
std::string fileName(databasePath);
- fullBody_->AddLimb(fileName, std::string(id), problemSolver_->collisionObstacles(), heuristicName, loadValues > 0.5);
+ fullBody_->AddLimb(fileName, std::string(id), problemSolver_->collisionObstacles(), heuristicName, loadValues > 0.5,
+ disableEffectorCollision > 0.5);
}
catch(std::runtime_error& e)
{
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 45062edbb81b248ee49da516f0c5bd61f849dfbb..a713fbbdc5cc02d429596b423af3a1d7cbddc36a 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -118,8 +118,10 @@ namespace hpp {
double octreeNodeId) throw (hpp::Error);
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 short samples, const char *heuristicName, double resolution, const char *contactType) throw (hpp::Error);
- virtual void addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues) throw (hpp::Error);
+ unsigned short samples, const char *heuristicName, double resolution, const char *contactType,
+ double disableEffectorCollision) throw (hpp::Error);
+ virtual void addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues,
+ double disableEffectorCollision) throw (hpp::Error);
virtual void setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);