From 3adcaf4df549100a309b69b0e75a0bb7a4aeeade Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Mon, 6 Jun 2016 15:32:32 +0200
Subject: [PATCH] allowing to disable end effector collision
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 7 +++-
script/scenarios/ground_crouch_hyq_interp.py | 43 ++++++++++----------
script/scenarios/ground_crouch_hyq_path.py | 4 +-
script/scenarios/stair_bauzil_hrp2_interp.py | 3 +-
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 16 ++++++--
src/rbprmbuilder.impl.cc | 10 +++--
src/rbprmbuilder.impl.hh | 6 ++-
7 files changed, 52 insertions(+), 37 deletions(-)
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 83810e73..fd201e87 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 2e60bca6..4ba8f75b 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 b5f1f81b..c3d48f92 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 0dc48b34..2f80a41b 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 77479a1b..2686320a 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 39bd42f2..a3840b98 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 45062edb..a713fbbd 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);
--
GitLab