diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 34164d2b842649abd50f98b3497351930f5bb71a..e69bb252f1a1b56640f1dd5cc8b74bb30e6f261a 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -176,14 +176,15 @@ module hpp
/// to this function. If these conditions are not met an error is raised.
/// \param timestep normalized step for generation along the path (ie the path has a length of 1).
/// \param path path computed.
- floatSeqSeq interpolate(in double timestep, in double path) raises (Error);
+ floatSeqSeq interpolate(in double timestep, in double path, in double robustnessTreshold) raises (Error);
/// Provided a path has already been computed, interpolates it and generates the statically stable
/// constact configurations along it. setStartState and setEndState must have been called prior
/// to this function. If these conditions are not met an error is raised.
/// \param timestep normalized step for generation along the path (ie the path has a length of 1).
/// \param path path computed.
- floatSeqSeq interpolateConfigs(in floatSeqSeq configs) raises (Error);
+ /// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
+ floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold) raises (Error);
/// Saves the last computed states by the function interpolate in a filename.
/// Raises an error if interpolate has not been called, or the file could not be opened.
diff --git a/script/scenarios/darpa_hyq_interp.py b/script/scenarios/darpa_hyq_interp.py
index e8c5562323c75efcfde3408cc98a99391f32ee98..704b61448d40cc78aaa8de10cd573efc56b317f1 100755
--- a/script/scenarios/darpa_hyq_interp.py
+++ b/script/scenarios/darpa_hyq_interp.py
@@ -34,8 +34,6 @@ 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.05)
-#~ fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "random", 0.05)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
@@ -44,8 +42,6 @@ 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)
-#~ fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "random", 0.05)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
@@ -54,8 +50,6 @@ 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)
-#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "random", 0.05)
larmId = 'lfleg'
larm = 'lf_haa_joint'
@@ -64,8 +58,6 @@ 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, "manipulability", 0.05)
-#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "random", 0.05)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
@@ -84,21 +76,12 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
-configs = fullBody.interpolate(0.1)
+configs = fullBody.interpolate(0.1, 1, 0)
r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
-#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
-#~ fullBody.createOctreeBoxes(r.client.gui, 1, larmId, q_0,)
i = 0;
fullBody.draw(configs[i],r); i=i+1; i-1
#~ fullBody.exportAll(r, configs, 'darpa_hyq_robust_1');
-#~ r (configs[i]); i=i+1; i-1
-
-#~ q0 = configs[2]
-#~ q0 = fullBody.generateContacts(q0, [0,0,1])
-#~ q_init [0:3] = [0, 0, 0.63]; r(q_init)
-#~ c = fullBody.getContactSamplesIds("rfleg",q_init, [1,0,0])
-#~ r(fullBody.getSample("rfleg",int(c[i]))); i = i+1
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index c3d4cc00cbe89162bf2ef9a3ec32618ca1775c09..14a1889d8e323ecd453a0776c335bdba500f9d95 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -173,8 +173,9 @@ class FullBody (object):
#
# \param stepSize discretization step
# \param pathId Id of the path to compute from
- def interpolate(self, stepsize, pathId = 1):
- return self.client.rbprm.rbprm.interpolate(stepsize, pathId)
+ # \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
+ def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0):
+ return self.client.rbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold)
## Given start and goal states
# generate a contact sequence over a list of configurations
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index ac245ec83df928f87d920f946353eaf5b12b686a..68434842d7b7576ac68afd6bcb6dc4991127ba93 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -428,7 +428,7 @@ namespace hpp {
}
}
- floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs) throw (hpp::Error)
+ floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error)
{
try
{
@@ -442,7 +442,7 @@ namespace hpp {
}
hpp::rbprm::RbPrmInterpolationPtr_t interpolator = rbprm::RbPrmInterpolation::create(fullBody_,startState_,endState_);
std::vector<model::Configuration_t> configurations = doubleDofArrayToConfig(fullBody_->device_, configs);
- lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),configurations);
+ lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),configurations,robustnessTreshold);
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
@@ -472,7 +472,7 @@ namespace hpp {
}
}
- floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path) throw (hpp::Error)
+ floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold) throw (hpp::Error)
{
try
{
@@ -492,16 +492,10 @@ namespace hpp {
}
hpp::rbprm::RbPrmInterpolationPtr_t interpolator = rbprm::RbPrmInterpolation::create(fullBody_,startState_,endState_,problemSolver_->paths()[pathId]);
- lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),timestep);
+ lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),timestep,robustnessTreshold);
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
- /*for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin(); cit != lastStatesComputed_.end(); ++cit)
- {
- const core::Configuration_t config = cit->configuration_;
- hpp::floatSeq dofArray;
- dofArray.length (config.size());
- }*/
res->length (lastStatesComputed_.size ());
std::size_t i=0;
@@ -568,8 +562,6 @@ namespace hpp {
fullBody_->device_->computeForwardKinematics();
const std::map<std::size_t, fcl::CollisionObject*>& boxes =
fullBody_->GetLimbs().at(std::string(limbName))->sampleContainer_.boxes_;
- //const hpp::rbprm::RbPrmLimbPtr_t limb =fullBody_->GetLimbs().at(std::string(limbName));
- //const fcl::Transform3f transform = limb->octreeRoot();
const double resolution = fullBody_->GetLimbs().at(std::string(limbName))->sampleContainer_.resolution_;
std::size_t i =0;
hpp::floatSeqSeq *res;
@@ -578,7 +570,7 @@ namespace hpp {
for(std::map<std::size_t, fcl::CollisionObject*>::const_iterator cit = boxes.begin();
cit != boxes.end();++cit,++i)
{
- fcl::Vec3f position = /*transform.getRotation() **/ (*cit->second).getTranslation() /*+ transform.getTranslation()*/;
+ fcl::Vec3f position = (*cit->second).getTranslation();
_CORBA_ULong size = (_CORBA_ULong) 4;
double* dofArray = hpp::floatSeq::allocbuf(size);
hpp::floatSeq floats (size, size, dofArray, true);
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index c48993661217bb145118f6f8e42eb2500cc56e62..0423c07c4e7e2a71ecea8ec90367a6c76d3bd6ec 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -114,8 +114,8 @@ namespace hpp {
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);
- virtual hpp::floatSeqSeq* interpolate(double timestep, double path) throw (hpp::Error);
- virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs) throw (hpp::Error);
+ virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold) throw (hpp::Error);
+ virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error);
virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
virtual hpp::floatSeqSeq* GetOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
virtual hpp::floatSeq* getOctreeTransform(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);