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

rebase

parent fdc63ef6
......@@ -242,7 +242,9 @@ class FullBody (object):
# \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).
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
# generate a contact sequence over a list of configurations
......
......@@ -661,20 +661,29 @@ namespace hpp {
const State& firstState = lastStatesComputed_[cId], thirdState = lastStatesComputed_[cId+1];
std::vector<std::vector<fcl::Vec3f> > allStates;
allStates.push_back(computeRectangleContact(fullBody_, firstState));
std::vector<std::string> variations =thirdState.contactVariations(firstState);
if(variations.size() >1)
std::vector<std::string> breaks, creations;
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)));
}
if(!variations.empty())
if(breaks.size() == 1)
{
std::cout << "variation " << variations[0] << std::endl;
State intermediary(firstState);
intermediary.RemoveContact(*variations.begin());
intermediary.RemoveContact(*breaks.begin());
allStates.push_back(computeRectangleContact(fullBody_, intermediary));
}
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;
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