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

add a projectStateToCom method that take a state as input and not an ID

parent 37cb84a6
...@@ -692,34 +692,43 @@ namespace hpp { ...@@ -692,34 +692,43 @@ namespace hpp {
return res; return res;
} }
double RbprmBuilder::projectStateToCOMEigen(unsigned short stateId, const model::Configuration_t& com_target, unsigned short maxNumeSamples) throw (hpp::Error) double RbprmBuilder::projectStateToCOMEigen(unsigned short stateId, const model::Configuration_t& com_target, unsigned short maxNumeSamples) throw (hpp::Error){
if(lastStatesComputed_.size() <= stateId)
{
throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
}
State s = lastStatesComputed_[stateId];
double success = projectStateToCOMEigen(s,com_target,maxNumeSamples);
lastStatesComputed_[stateId] = s;
lastStatesComputedTime_[stateId].second = s;
return success;
}
double RbprmBuilder::projectStateToCOMEigen(State& s, const model::Configuration_t& com_target, unsigned short maxNumeSamples) throw (hpp::Error)
{ {
try try
{ {
if(lastStatesComputed_.size() <= stateId)
{
throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
}
State s = lastStatesComputed_[stateId];
// /hpp::model::Configuration_t c = rbprm::interpolation::projectOnCom(fullBody(), problemSolver()->problem(),s,com_target,succes); // /hpp::model::Configuration_t c = rbprm::interpolation::projectOnCom(fullBody(), problemSolver()->problem(),s,com_target,succes);
rbprm::projection::ProjectionReport rep = rbprm::projection::projectToComPosition(fullBody(),com_target,s); rbprm::projection::ProjectionReport rep = rbprm::projection::projectToComPosition(fullBody(),com_target,s);
hpp::model::Configuration_t& c = rep.result_.configuration_; hpp::model::Configuration_t& c = rep.result_.configuration_;
ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport)); ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
CollisionValidationPtr_t val = fullBody()->GetCollisionValidation(); CollisionValidationPtr_t val = fullBody()->GetCollisionValidation();
if(!rep.success_){ if(!rep.success_){
hppDout(notice,"Projection failed for state "<<stateId<<" , config = "<<model::displayConfig(c)); hppDout(notice,"Projection failed for state with config = "<<model::displayConfig(c));
} }
if(rep.success_){ if(rep.success_){
hppDout(notice,"Projection successfull for state "<<stateId<<" without collision check."); hppDout(notice,"Projection successfull for state without collision check.");
rep.success_ = rep.success_ && val->validate(rep.result_.configuration_,rport); rep.success_ = rep.success_ && val->validate(rep.result_.configuration_,rport);
if(!rep.success_){ if(!rep.success_){
hppDout(notice,"Projection failed after collision check for state "<<stateId<<" , config = "<<model::displayConfig(c)); hppDout(notice,"Projection failed after collision check for state with config = "<<model::displayConfig(c));
hppDout(notice,"report : "<<*rport); hppDout(notice,"report : "<<*rport);
}else{
hppDout(notice,"Success after collision check");
} }
} }
if (! rep.success_ && maxNumeSamples>0) if (! rep.success_ && maxNumeSamples>0)
{ {
hppDout(notice,"Projection for state "<<stateId<<" failed, try to randomly sample other initial point : "); hppDout(notice,"Projection for state failed, try to randomly sample other initial point : ");
Configuration_t head = s.configuration_.head<7>(); Configuration_t head = s.configuration_.head<7>();
size_t ecs_size = fullBody()->device_->extraConfigSpace().dimension (); size_t ecs_size = fullBody()->device_->extraConfigSpace().dimension ();
Configuration_t ecs = s.configuration_.tail(ecs_size); Configuration_t ecs = s.configuration_.tail(ecs_size);
...@@ -730,16 +739,20 @@ namespace hpp { ...@@ -730,16 +739,20 @@ namespace hpp {
s.configuration_.head<7>() = head; s.configuration_.head<7>() = head;
s.configuration_.tail(ecs_size) = ecs; s.configuration_.tail(ecs_size) = ecs;
//c = rbprm::interpolation::projectOnCom(fullBody(), problemSolver()->problem(),s,com_target,succes); //c = rbprm::interpolation::projectOnCom(fullBody(), problemSolver()->problem(),s,com_target,succes);
hppDout(notice,"Sample before prjection : r(["<<model::displayConfig(s.configuration_)<<"])");
rep = rbprm::projection::projectToComPosition(fullBody(),com_target,s); rep = rbprm::projection::projectToComPosition(fullBody(),com_target,s);
hppDout(notice,"Sample after prjection : r(["<<model::displayConfig(rep.result_.configuration_)<<"])");
if(!rep.success_){ if(!rep.success_){
hppDout(notice,"Projection failed on iter "<<i<<" for state "<<stateId<<" , config = "<<model::displayConfig(c)); hppDout(notice,"Projection failed on iter "<<i);
} }
if(rep.success_){ if(rep.success_){
hppDout(notice,"Projection successfull on iter "<<i<<" for state "<<stateId<<" without collision check."); hppDout(notice,"Projection successfull on iter "<<i<<" without collision check.");
rep.success_ = rep.success_ && val->validate(rep.result_.configuration_,rport); rep.success_ = rep.success_ && val->validate(rep.result_.configuration_,rport);
if(!rep.success_){ if(!rep.success_){
hppDout(notice,"Projection failed on iter "<<i<<" after collision check for state "<<stateId<<" , config = "<<model::displayConfig(c)); hppDout(notice,"Projection failed on iter "<<i<<" after collision check");
hppDout(notice,"report : "<<*rport); hppDout(notice,"report : "<<*rport);
}else{
hppDout(notice,"Success after collision check");
} }
} }
c = rep.result_.configuration_; c = rep.result_.configuration_;
...@@ -747,6 +760,7 @@ namespace hpp { ...@@ -747,6 +760,7 @@ namespace hpp {
} }
if(rep.success_) if(rep.success_)
{ {
hppDout(notice,"Valid configuration found after projection : r(["<<model::displayConfig(c)<<"])");
hpp::model::Configuration_t trySave = c; hpp::model::Configuration_t trySave = c;
rbprm::T_Limb fLimbs = GetFreeLimbs(fullBody(), s); rbprm::T_Limb fLimbs = GetFreeLimbs(fullBody(), s);
for(rbprm::CIT_Limb cit = fLimbs.begin(); cit != fLimbs.end() && rep.success_; ++cit) for(rbprm::CIT_Limb cit = fLimbs.begin(); cit != fLimbs.end() && rep.success_; ++cit)
...@@ -767,10 +781,12 @@ namespace hpp { ...@@ -767,10 +781,12 @@ namespace hpp {
std::cout << "ow" << std::endl; std::cout << "ow" << std::endl;
} }
} }
lastStatesComputed_[stateId].configuration_ = trySave; s.configuration_ = trySave;
lastStatesComputedTime_[stateId].second.configuration_ = trySave; hppDout(notice,"ProjectoToComEigen success");
return 1.; return 1.;
} }
hppDout(notice,"No valid configurations can be found after projection : r(["<<model::displayConfig(c)<<"])");
hppDout(notice,"ProjectoToComEigen failure");
return 0; return 0;
} }
catch(std::runtime_error& e) catch(std::runtime_error& e)
...@@ -806,11 +822,12 @@ namespace hpp { ...@@ -806,11 +822,12 @@ namespace hpp {
} }
double RbprmBuilder::projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error) double RbprmBuilder::projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error)
{ {
model::Configuration_t com_target = dofArrayToConfig (3, com); model::Configuration_t com_target = dofArrayToConfig (3, com);
return projectStateToCOMEigen(stateId, com_target, max_num_sample); return projectStateToCOMEigen(stateId, com_target, max_num_sample);
} }
rbprm::State RbprmBuilder::generateContacts_internal(const hpp::floatSeq& configuration, rbprm::State RbprmBuilder::generateContacts_internal(const hpp::floatSeq& configuration,
const hpp::floatSeq& direction,const hpp::floatSeq& acceleration, const double robustnessThreshold ) throw (hpp::Error) const hpp::floatSeq& direction,const hpp::floatSeq& acceleration, const double robustnessThreshold ) 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