From a10fc4360f0ba24e0d3bb16159248a048b2ed8e6 Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Thu, 7 Jan 2016 11:59:22 +0100
Subject: [PATCH] added contact types: 3 dof for hyq, 6 for hrp2
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 4 +++-
script/scenarios/darpa_hyq_interp.py | 10 +++++-----
script/scenarios/ground_crouch_hyq_interp.py | 9 +++++----
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 5 +++--
src/rbprmbuilder.impl.cc | 9 +++++++--
src/rbprmbuilder.impl.hh | 2 +-
6 files changed, 24 insertions(+), 15 deletions(-)
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index e69bb252..d7876a70 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -155,8 +155,10 @@ module hpp
/// \param samples number of samples to generate for the limb (a typical value is 10000)
/// \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")
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) raises (Error);
+ in double x, in double y, in unsigned short samples, in string heuristicName,
+ in double resolution, in string contactType) raises (Error);
/// Set the start state of a contact generation problem
/// environment, ordered by their efficiency
diff --git a/script/scenarios/darpa_hyq_interp.py b/script/scenarios/darpa_hyq_interp.py
index 704b6144..4b5301e3 100755
--- a/script/scenarios/darpa_hyq_interp.py
+++ b/script/scenarios/darpa_hyq_interp.py
@@ -26,14 +26,14 @@ r = tp.Viewer (ps)
rootName = 'base_joint_xyz'
-#~ AFTER loading obstacles
+cType = "_3_DOF"
rLegId = 'rfleg'
rLeg = 'rf_haa_joint'
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, "manipulability", 0.1)
+fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "manipulability", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
@@ -41,7 +41,7 @@ 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, "manipulability", 0.05)
+fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "manipulability", 0.05, cType)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
@@ -49,7 +49,7 @@ 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, "manipulability", 0.05)
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "manipulability", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
@@ -57,7 +57,7 @@ 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)
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "forward", 0.05, cType)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
diff --git a/script/scenarios/ground_crouch_hyq_interp.py b/script/scenarios/ground_crouch_hyq_interp.py
index 4d23bb17..2e60bca6 100755
--- a/script/scenarios/ground_crouch_hyq_interp.py
+++ b/script/scenarios/ground_crouch_hyq_interp.py
@@ -27,13 +27,14 @@ r = tp.Viewer (ps)
rootName = 'base_joint_xyz'
#~ AFTER loading obstacles
+cType = "_3_DOF"
rLegId = 'rfleg'
rLeg = 'rf_haa_joint'
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)
+fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "forward", 0.1,cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
@@ -41,7 +42,7 @@ 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)
+fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "backward", 0.05,cType)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
@@ -49,7 +50,7 @@ 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)
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "backward", 0.05,cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
@@ -57,7 +58,7 @@ 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)
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "forward", 0.05,cType)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 14a1889d..f068c589 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -110,8 +110,9 @@ class FullBody (object):
# structure to perform efficient proximity requests. The resulution of the octree, in meters, specifies the size
# 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.
- def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution):
- self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution)
+ # \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)
## Returns the configuration of a limb described by a sample
#
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 68434842..be1d8e7f 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -353,7 +353,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) throw (hpp::Error)
+ unsigned short samples, const char* heuristicName, double resolution, const char *contactType) throw (hpp::Error)
{
if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded");
@@ -365,7 +365,12 @@ namespace hpp {
off[i] = offset[i];
norm[i] = normal[i];
}
- fullBody_->AddLimb(std::string(id), std::string(limb), std::string(effector), off, norm, x, y, problemSolver_->collisionObstacles(), samples,heuristicName,resolution);
+ ContactType cType = hpp::rbprm::_6_DOF;
+ if(std::string(contactType) == "_3_DOF")
+ {
+ 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);
}
catch(std::runtime_error& e)
{
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 0423c07c..a7db5119 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -110,7 +110,7 @@ namespace hpp {
const hpp::floatSeq& direction) 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) throw (hpp::Error);
+ unsigned short samples, const char *heuristicName, double resolution, const char *contactType) 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