diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl index a14fb354feb97311028adf514990a481b53d25c9..cadf322cc45ab8baa1ec8c7763ed89c9d3370f7f 100755 --- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl +++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl @@ -185,7 +185,7 @@ module hpp /// \param dofArray initial configuration of the robot /// \param direction desired direction of motion for the robot /// \return transformed configuration for which all possible contacts have been created - floatSeq generateContacts(in floatSeq dofArray, in floatSeq direction, in floatSeq acceleration) raises (Error); + floatSeq generateContacts(in floatSeq dofArray, in floatSeq direction, in floatSeq acceleration, in double robustnessThreshold) raises (Error); /// Generate an autocollision free configuration with limbs in contact with the ground /// \param contactLimbs name of the limbs to project in contact diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py index 64ba9166f00b32dd8945b0c43728441e7d438f62..f250dc22ada8d41d71dca82530f6e1c8ea7df7ce 100755 --- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py +++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py @@ -198,11 +198,11 @@ class FullBody (object): ## Generates a balanced contact configuration, considering the # given current configuration of the robot, and a direction of motion. # Typically used to generate a start and / or goal configuration automatically for a planning problem. - # + # # \param configuration the initial robot configuration # \param direction a 3d vector specifying the desired direction of motion - def generateContacts(self, configuration, direction,acceleration = [0,0,0]): - return self.client.rbprm.rbprm.generateContacts(configuration, direction, acceleration) + def generateContacts(self, configuration, direction,acceleration = [0,0,0], robustnessThreshold = 0): + return self.client.rbprm.rbprm.generateContacts(configuration, direction, acceleration, robustnessThreshold) ## Generate an autocollision free configuration with limbs in contact with the ground # \param contactLimbs name of the limbs to project in contact diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc index 5a3ba5b9dcff2700fa6d182b42f35cba57c25f6d..4c5cb21f84762b4b27a054106dd76eb065a1bdf7 100755 --- a/src/rbprmbuilder.impl.cc +++ b/src/rbprmbuilder.impl.cc @@ -655,7 +655,7 @@ namespace hpp { } hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration, - const hpp::floatSeq& direction,const hpp::floatSeq& acceleration) throw (hpp::Error) + const hpp::floatSeq& direction,const hpp::floatSeq& acceleration, const double robustnessThreshold ) throw (hpp::Error) { if(!fullBodyLoaded_) throw Error ("No full body robot was loaded"); @@ -675,7 +675,7 @@ namespace hpp { model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration); rbprm::State state = rbprm::ComputeContacts(fullBody_,config, - affMap, bindShooter_.affFilter_, dir,0,acc); + affMap, bindShooter_.affFilter_, dir,robustnessThreshold,acc); hpp::floatSeq* dofArray = new hpp::floatSeq(); dofArray->length(_CORBA_ULong(state.configuration_.rows())); for(std::size_t i=0; i< _CORBA_ULong(config.rows()); i++) diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh index 308933f8c6286fcd5c04b7271ee91daf36def3f6..1bae4868ec48134d6bbb11e591b2f3c44b7215ce 100755 --- a/src/rbprmbuilder.impl.hh +++ b/src/rbprmbuilder.impl.hh @@ -131,7 +131,7 @@ namespace hpp { virtual double getEffectorDistance(unsigned short state1, unsigned short state2) throw (hpp::Error); virtual hpp::floatSeq* generateContacts(const hpp::floatSeq& configuration, - const hpp::floatSeq& direction, const hpp::floatSeq& acceleration) throw (hpp::Error); + const hpp::floatSeq& direction, const hpp::floatSeq& acceleration, const double robustnessThreshold) throw (hpp::Error); virtual hpp::floatSeq* generateGroundContact(const hpp::Names_t& contactLimbs) throw (hpp::Error);