Commit e43e79d8 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

in comRRT methods : use projectStateToCOm instead of project_or_throw

parent d2d0f01a
...@@ -2309,8 +2309,13 @@ assert(s2 == s1 +1); ...@@ -2309,8 +2309,13 @@ assert(s2 == s1 +1);
State s1Bis(state1); State s1Bis(state1);
hppDout(notice,"state1 = "<<model::displayConfig(state1.configuration_)); hppDout(notice,"state1 = "<<model::displayConfig(state1.configuration_));
hppDout(notice,"proj to CoM position : "<<paths[cT1]->end().head<3>()); hppDout(notice,"proj to CoM position : "<<paths[cT1]->end().head<3>());
s1Bis.configuration_ = project_or_throw(fullBody(),s1Bis,paths[cT1]->end().head<3>(),true); bool successProjS1 = (projectStateToCOMEigen(s1Bis,paths[cT1]->end().head<3>(),100) > 0.5 );
hppDout(notice,"state1 after projection= "<<model::displayConfig(s1Bis.configuration_)); if(!successProjS1)
{
hppDout(notice,"could not project state S1Bis on COM constraint");
throw std::runtime_error("could not project state S1Bis on COM constraint");
}
hppDout(notice,"state1 bis after projection= "<<model::displayConfig(s1Bis.configuration_));
for(std::map<std::string,bool>::const_iterator cit = s1Bis.contacts_.begin();cit!=s1Bis.contacts_.end(); ++ cit) for(std::map<std::string,bool>::const_iterator cit = s1Bis.contacts_.begin();cit!=s1Bis.contacts_.end(); ++ cit)
{ {
...@@ -2320,56 +2325,26 @@ assert(s2 == s1 +1); ...@@ -2320,56 +2325,26 @@ assert(s2 == s1 +1);
State s2Bis(state2); State s2Bis(state2);
hppDout(notice,"state2 = "<<model::displayConfig(state2.configuration_)); hppDout(notice,"state2 = "<<model::displayConfig(state2.configuration_));
hppDout(notice,"proj to CoM position : "<<paths[cT2]->end().head<3>()); hppDout(notice,"proj to CoM position : "<<paths[cT2]->end().head<3>());
s2Bis.configuration_ = project_or_throw(fullBody(), s2Bis,paths[cT2]->end().head<3>(), true); bool successProjS2 = (projectStateToCOMEigen(s2Bis,paths[cT2]->end().head<3>(),100) > 0.5 );
hppDout(notice,"state2 after projection= "<<model::displayConfig(s2Bis.configuration_)); if(!successProjS2)
for(std::map<std::string,bool>::const_iterator cit = s2Bis.contacts_.begin();cit!=s2Bis.contacts_.end(); ++ cit)
{ {
hppDout(notice,"contact : "<<cit->first<<" = "<<cit->second); hppDout(notice,"could not project state S2Bis on COM constraint");
throw std::runtime_error("could not project state S2Bis on COM constraint");
} }
hppDout(notice,"state2 bis after projection= "<<model::displayConfig(s2Bis.configuration_));
ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
BasicConfigurationShooterPtr_t shooter = BasicConfigurationShooter::create(fullBody()->device_); for(std::map<std::string,bool>::const_iterator cit = s2Bis.contacts_.begin();cit!=s2Bis.contacts_.end(); ++ cit)
bool found = false;
for (int i = 0; i< 100 && !found; ++i)
{
fullBody()->device_->currentConfiguration(s1Bis.configuration_);
found =problemSolver()->problem()->configValidations()->validate(s1Bis.configuration_, rport);
if(!found)
{
std::cout << "could not project s1Bis without collision at state " << s1 << std::endl;
std::cout << "collission " << *rport << std::endl;
s1Bis.configuration_ = *shooter->shoot();
s1Bis.configuration_ = project_or_throw(fullBody(), s1Bis,paths[cT1]->end().head<3>());
std::cout << "projection succeedded " << paths[cT1]->end().head<3>() << std::endl;
}
}
if(found)
std::cout << "got out ! " << std::endl;
bool found2 = false;
for (int i = 0; i< 100 && found && !found2; ++i)
{
fullBody()->device_->currentConfiguration(s2Bis.configuration_);
found2 =problemSolver()->problem()->configValidations()->validate(s2Bis.configuration_, rport);
if(!found2)
{
std::cout << "could not project s2Bis without collision at state " << s1 << std::endl;
std::cout << "collission " << *rport << std::endl;
s2Bis.configuration_ = *shooter->shoot();
s2Bis.configuration_ = project_or_throw(fullBody(), s2Bis,paths[cT2]->end().head<3>());
std::cout << "projection succeedded " << paths[cT2]->end().head<3>() << std::endl;
}
}
if(!found || !found2)
{ {
std::cout << "could not project without collision at state " << s1 << std::endl; hppDout(notice,"contact : "<<cit->first<<" = "<<cit->second);
throw std::runtime_error ("could not project without collision at state " + s1 );
} }
core::PathVectorPtr_t resPath = core::PathVector::create(fullBody()->device_->configSize(), fullBody()->device_->numberDof()); core::PathVectorPtr_t resPath = core::PathVector::create(fullBody()->device_->configSize(), fullBody()->device_->numberDof());
hppDout(notice,"ComRRT : projections done. begin rrt"); hppDout(notice,"ComRRT : projections done. begin rrt");
try{ try{
hppDout(notice,"begin comRRT states 1 and 1bis"); hppDout(notice,"begin comRRT states 1 and 1bis");
hppDout(notice,"state1 = r(["<<model::displayConfig(state1.configuration_)<<"])");
hppDout(notice,"state1 bis = r(["<<model::displayConfig(s1Bis.configuration_)<<"])");
core::PathPtr_t p1 = interpolation::comRRT(fullBody(),problemSolver()->problem(), paths[cT1], core::PathPtr_t p1 = interpolation::comRRT(fullBody(),problemSolver()->problem(), paths[cT1],
state1,s1Bis, numOptimizations,true); state1,s1Bis, numOptimizations,true);
hppDout(notice,"end comRRT"); hppDout(notice,"end comRRT");
......
...@@ -331,7 +331,9 @@ namespace hpp { ...@@ -331,7 +331,9 @@ namespace hpp {
virtual CORBA::Short createState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error); virtual CORBA::Short createState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual hpp::floatSeq* getConfigAtState(unsigned short stateId) throw (hpp::Error); virtual hpp::floatSeq* getConfigAtState(unsigned short stateId) throw (hpp::Error);
virtual double setConfigAtState(unsigned short stateId, const hpp::floatSeq& config) throw (hpp::Error); virtual double setConfigAtState(unsigned short stateId, const hpp::floatSeq& config) throw (hpp::Error);
double projectStateToCOMEigen(State& s, const model::Configuration_t& com_target, unsigned short maxNumeSamples)throw (hpp::Error);
double projectStateToCOMEigen(unsigned short stateId, const model::Configuration_t& com_target, unsigned short maxNumeSamples)throw (hpp::Error); double projectStateToCOMEigen(unsigned short stateId, const model::Configuration_t& com_target, unsigned short maxNumeSamples)throw (hpp::Error);
virtual double projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error); virtual double projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error);
virtual void saveComputedStates(const char* filepath) throw (hpp::Error); virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
virtual void saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error); virtual void saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error);
......
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