Commit 63e2f2f0 authored by stevet's avatar stevet
Browse files

[Feature] States not necessarily deleted when interpolation is called

parent 31d3c8cb
......@@ -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.
......
......@@ -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.
......
......@@ -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
{
......
......@@ -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);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment