From 63e2f2f0d8dfa1628be67719d759700d5393ddc5 Mon Sep 17 00:00:00 2001
From: stevet <stevetonneau@gotmail.fr>
Date: Wed, 7 Aug 2019 10:40:14 +0200
Subject: [PATCH] [Feature] States not necessarily deleted when interpolation
is called
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 8 +++-
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 10 ++++-
src/rbprmbuilder.impl.cc | 50 +++++++++++++++++-----
src/rbprmbuilder.impl.hh | 5 ++-
4 files changed, 56 insertions(+), 17 deletions(-)
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index bd6ce9e9..bafa1eb2 100644
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -371,7 +371,7 @@ module hpp
/// \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
/// \param testReachability : if true, check each contact transition with our reachability criterion
/// \param quasiStatic : if True, use our reachability criterion with the quasiStatic constraint
- floatSeqSeq interpolate(in double timestep, in double path, in double robustnessTreshold, in unsigned short filterStates, in boolean testReachability, in boolean quasiStatic) raises (Error);
+ floatSeqSeq interpolate(in double timestep, in double path, in double robustnessTreshold, in unsigned short filterStates, in boolean testReachability, in boolean quasiStatic, in boolean erasePreviousStates) 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
@@ -382,7 +382,7 @@ module hpp
/// \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
/// \param testReachability : if true, check each contact transition with our reachability criterion
/// \param quasiStatic : if True, use our reachability criterion with the quasiStatic constraint
- floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold, in unsigned short filterStates,in boolean testReachability, in boolean quasiStatic) raises (Error);
+ floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold, in unsigned short filterStates,in boolean testReachability, in boolean quasiStatic, in boolean erasePreviousStates) raises (Error);
/// returns the CWC of the robot at a given state
@@ -670,6 +670,10 @@ module hpp
/// \return whether the limb is in contact at this state
short computeIntermediary(in unsigned short state1, in unsigned short state2) raises (Error);
+ /// Compute the number of computed states
+ /// \return the number of computed states
+ short getNumStates() 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.
/// \param filename name of the file used to save the contacts.
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index e1b702ca..a76d9c8a 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -257,6 +257,11 @@ class FullBody (Robot):
def getNumSamples(self, limbName):
return self.clientRbprm.rbprm.getNumSamples(limbName)
+ ## Get the number of existing states in the client
+ #
+ def getNumStates(self, limbName):
+ return self.clientRbprm.rbprm.getNumStates()
+
## Get the number of octreeNodes
#
# \param limbName name of the limb from which to retrieve a sample
@@ -348,12 +353,13 @@ class FullBody (Robot):
# \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
# \param testReachability : if true, check each contact transition with our reachability criterion
# \param quasiStatic : if True, use our reachability criterion with the quasiStatic constraint
- def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0, filterStates = False,testReachability = True, quasiStatic = False):
+ # \param erasePreviousStates : if True the list of previously computed states is erased
+ def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0, filterStates = False,testReachability = True, quasiStatic = False, erasePreviousStates = True):
if(filterStates):
filt = 1
else:
filt = 0
- return self.clientRbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold, filt,testReachability, quasiStatic)
+ return self.clientRbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold, filt,testReachability, quasiStatic, erasePreviousStates)
## Provided a discrete contact sequence has already been computed, computes
# all the contact positions and normals for a given state, the next one, and the intermediate between them.
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index ac4dc272..efd80e0f 100644
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -1544,7 +1544,7 @@ namespace hpp {
return res;
}
- floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic) throw (hpp::Error)
+ floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic, bool erasePreviousStates) throw (hpp::Error)
{
try
{
@@ -1563,15 +1563,16 @@ namespace hpp {
throw hpp::Error ("No affordances found. Unable to interpolate.");
}
hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = rbprm::interpolation::RbPrmInterpolation::create(fullBody(),startState_,endState_,core::PathVectorConstPtr_t(),testReachability,quasiStatic);
- lastStatesComputedTime_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,configurations,robustnessTreshold, filterStates != 0);
- lastStatesComputed_ = TimeStatesToStates(lastStatesComputedTime_);
+ rbprm::T_StateFrame newTimeStates =
+ interpolator->Interpolate(affMap, bindShooter_.affFilter_,configurations,robustnessTreshold, filterStates != 0);
+ std::vector<rbprm::State> newStates =TimeStatesToStates(newTimeStates);
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
- res->length ((_CORBA_ULong)lastStatesComputed_.size ());
+ res->length ((_CORBA_ULong)newStates.size ());
std::size_t i=0;
std::size_t id = 0;
- for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin(); cit != lastStatesComputed_.end(); ++cit, ++id)
+ for(std::vector<State>::const_iterator cit = newStates.begin(); cit != newStates.end(); ++cit, ++id)
{
std::cout << "ID " << id;
cit->print();
@@ -1586,6 +1587,16 @@ namespace hpp {
(*res) [(_CORBA_ULong)i] = floats;
++i;
}
+ if(erasePreviousStates)
+ {
+ lastStatesComputedTime_ = newTimeStates;
+ lastStatesComputed_ = newStates;
+ }
+ else
+ {
+ lastStatesComputed_.insert(lastStatesComputed_.begin(), newStates.begin(), newStates.end());
+ lastStatesComputedTime_.insert(lastStatesComputedTime_.begin(), newTimeStates.begin(), newTimeStates.end());
+ }
return res;
}
catch(std::runtime_error& e)
@@ -2233,7 +2244,7 @@ namespace hpp {
}
- floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic) throw (hpp::Error)
+ floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic, bool erasePreviousStates) throw (hpp::Error)
{
hppDout(notice,"### Begin interpolate");
try
@@ -2260,18 +2271,19 @@ namespace hpp {
hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator =
rbprm::interpolation::RbPrmInterpolation::create(fullBody(),startState_,endState_,problemSolver()->paths()[pathId],testReachability,quasiStatic);
- lastStatesComputedTime_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,
+
+ rbprm::T_StateFrame newTimeStates = interpolator->Interpolate(affMap, bindShooter_.affFilter_,
timestep,robustnessTreshold, filterStates != 0);
- lastStatesComputed_ = TimeStatesToStates(lastStatesComputedTime_);
+ std::vector<rbprm::State> newStates = TimeStatesToStates(newTimeStates);
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
- res->length ((_CORBA_ULong)lastStatesComputed_.size ());
+ res->length ((_CORBA_ULong)newStates.size ());
std::size_t i=0;
std::size_t id = 0;
- for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin();
- cit != lastStatesComputed_.end(); ++cit, ++id)
+ for(std::vector<State>::const_iterator cit = newStates.begin();
+ cit != newStates.end(); ++cit, ++id)
{
/*std::cout << "ID " << id;
cit->print();*/
@@ -2286,6 +2298,16 @@ namespace hpp {
(*res) [(_CORBA_ULong)i] = floats;
++i;
}
+ if(erasePreviousStates)
+ {
+ lastStatesComputedTime_ = newTimeStates;
+ lastStatesComputed_ = newStates;
+ }
+ else
+ {
+ lastStatesComputed_.insert(lastStatesComputed_.begin(), newStates.begin(), newStates.end());
+ lastStatesComputedTime_.insert(lastStatesComputedTime_.begin(), newTimeStates.begin(), newTimeStates.end());
+ }
return res;
}
catch(std::runtime_error& e)
@@ -3051,6 +3073,12 @@ namespace hpp {
}
+
+ CORBA::Short RbprmBuilder::getNumStates() throw (hpp::Error)
+ {
+ return lastStatesComputed_.size();
+ }
+
CORBA::Short RbprmBuilder::computeIntermediary(unsigned short stateFrom, unsigned short stateTo) throw (hpp::Error)
try
{
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index b7f07336..917922a0 100644
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -261,8 +261,8 @@ namespace hpp {
virtual hpp::floatSeqSeq* computeContactPointsForLimb(unsigned short cId, const char* limbName) throw (hpp::Error);
virtual hpp::floatSeqSeq* computeContactPointsAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char* limbName) throw (hpp::Error);
virtual hpp::floatSeqSeq* computeCenterOfContactAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char *limbName) throw (hpp::Error);
- virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic) throw (hpp::Error);
- virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic) throw (hpp::Error);
+ virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic, bool erasePreviousStates) throw (hpp::Error);
+ virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic, bool erasePreviousStates) throw (hpp::Error);
virtual hpp::floatSeqSeq* getContactCone(unsigned short stateId, double friction) throw (hpp::Error);
virtual hpp::floatSeqSeq* getContactIntermediateCone(unsigned short stateId, double friction) throw (hpp::Error);
virtual CORBA::Short generateComTraj(const hpp::floatSeqSeq& positions, const hpp::floatSeqSeq& velocities,
@@ -346,6 +346,7 @@ namespace hpp {
virtual CORBA::Short isLimbInContact(const char* limbName, unsigned short state) throw (hpp::Error);
virtual CORBA::Short isLimbInContactIntermediary(const char* limbName, unsigned short state) throw (hpp::Error);
virtual CORBA::Short computeIntermediary(unsigned short state1, unsigned short state2) throw (hpp::Error);
+ virtual CORBA::Short getNumStates() 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);
virtual CORBA::Short isConfigBalanced(const hpp::floatSeq& config, const hpp::Names_t& contactLimbs, double robustnessTreshold,const hpp::floatSeq& CoM) throw (hpp::Error);
--
GitLab