Commit 39a0cae9 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

rebase

parent fdc63ef6
...@@ -242,7 +242,9 @@ class FullBody (object): ...@@ -242,7 +242,9 @@ class FullBody (object):
# \param pathId Id of the path to compute from # \param pathId Id of the path to compute from
# \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default). # \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
def computeContactPoints(self, stateId): def computeContactPoints(self, stateId):
return self.client.rbprm.rbprm.computeContactPoints(stateId) rawdata = self.client.rbprm.rbprm.computeContactPoints(stateId)
return [[b[i:i+6] for i in range(0, len(b), 6)] for b in rawdata]
## Given start and goal states ## Given start and goal states
# generate a contact sequence over a list of configurations # generate a contact sequence over a list of configurations
......
...@@ -661,20 +661,29 @@ namespace hpp { ...@@ -661,20 +661,29 @@ namespace hpp {
const State& firstState = lastStatesComputed_[cId], thirdState = lastStatesComputed_[cId+1]; const State& firstState = lastStatesComputed_[cId], thirdState = lastStatesComputed_[cId+1];
std::vector<std::vector<fcl::Vec3f> > allStates; std::vector<std::vector<fcl::Vec3f> > allStates;
allStates.push_back(computeRectangleContact(fullBody_, firstState)); allStates.push_back(computeRectangleContact(fullBody_, firstState));
std::vector<std::string> variations =thirdState.contactVariations(firstState); std::vector<std::string> breaks, creations;
if(variations.size() >1) thirdState.contactBreaks(firstState, breaks);
if(breaks.size() > 1)
{ {
throw std::runtime_error ("too many state variation between states" + std::string(""+cId) + throw std::runtime_error ("too many contact breaks between states" + std::string(""+cId) +
"and " + std::string(""+(cId + 1))); "and " + std::string(""+(cId + 1)));
} }
if(!variations.empty()) if(breaks.size() == 1)
{ {
std::cout << "variation " << variations[0] << std::endl;
State intermediary(firstState); State intermediary(firstState);
intermediary.RemoveContact(*variations.begin()); intermediary.RemoveContact(*breaks.begin());
allStates.push_back(computeRectangleContact(fullBody_, intermediary)); allStates.push_back(computeRectangleContact(fullBody_, intermediary));
} }
allStates.push_back(computeRectangleContact(fullBody_, thirdState)); thirdState.contactCreations(firstState, creations);
if(creations.size() == 1)
{
allStates.push_back(computeRectangleContact(fullBody_, thirdState));
}
if(creations.size() > 1)
{
throw std::runtime_error ("too many contact creations between states" + std::string(""+cId) +
"and " + std::string(""+(cId + 1)));
}
hpp::floatSeqSeq *res; hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq (); res = new hpp::floatSeqSeq ();
......
Supports Markdown
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