diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 01b6ae8ed217393010f3876cfa323f1be4f0725e..7e299a3cf9a9dc91566c5e74f487604a9bdedfea 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -208,6 +208,12 @@ module hpp
/// \return transformed configuration for which all possible contacts have been created
floatSeq generateContacts(in floatSeq dofArray, in floatSeq direction, in floatSeq acceleration, in double robustnessThreshold) raises (Error);
+ /// Generate all possible contact in a given configuration and add the state in fullBody
+ /// \param dofArray initial configuration of the robot
+ /// \param direction desired direction of motion for the robot
+ /// \return the Id of the new state
+ short generateStateInContact(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
/// \return a sampled contact configuration
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 72f54a63eefd0f68740f4ece9fb3f390de088cd2..513fdfe49c5bcaeccb10151bc7383c3921d19500 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -223,9 +223,21 @@ class FullBody (object):
#
# \param configuration the initial robot configuration
# \param direction a 3d vector specifying the desired direction of motion
+ # \return the configuration in contact
def generateContacts(self, configuration, direction,acceleration = [0,0,0], robustnessThreshold = 0):
return self.client.rbprm.rbprm.generateContacts(configuration, direction, acceleration, robustnessThreshold)
+ ## 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
+ # \return the Id of the new state
+ def generateStateInContact(self, configuration, direction,acceleration = [0,0,0], robustnessThreshold = 0):
+ return self.client.rbprm.rbprm.generateStateInContact(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
# \return a sampled contact configuration
@@ -290,7 +302,7 @@ class FullBody (object):
# \param contacts the array of limbs in contact
def setStartState(self, configuration, contacts):
return self.client.rbprm.rbprm.setStartState(configuration, contacts)
-
+
## Create a state given a configuration and contacts
#
# \param configuration the desired start configuration
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 80410c02a31fba3ab05e53f5c4e812db8a9318e2..480144d663fb8bf6ae2ac6b9fb99e81932a797ee 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -808,7 +808,7 @@ namespace hpp {
return projectStateToCOMEigen(stateId, com_target, max_num_sample);
}
- hpp::floatSeq* RbprmBuilder::generateContacts(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)
{
if(!fullBodyLoaded_)
@@ -826,13 +826,25 @@ namespace hpp {
const affMap_t &affMap = problemSolver()->map<std::vector<boost::shared_ptr<model::CollisionObject> > > ();
if (affMap.empty ()) {
throw hpp::Error ("No affordances found. Unable to generate Contacts.");
- }
+ }
model::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration);
rbprm::State state = rbprm::contact::ComputeContacts(fullBody(),config,
affMap, bindShooter_.affFilter_, dir,robustnessThreshold,acc);
+ return state;
+ } catch (const std::exception& exc) {
+ throw hpp::Error (exc.what ());
+ }
+ }
+
+ hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration,
+ const hpp::floatSeq& direction,const hpp::floatSeq& acceleration, const double robustnessThreshold ) throw (hpp::Error)
+ {
+ try
+ {
+ rbprm::State state = generateContacts_internal(configuration,direction,acceleration,robustnessThreshold);
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++)
+ for(std::size_t i=0; i< _CORBA_ULong(state.configuration_.rows()); i++)
(*dofArray)[(_CORBA_ULong)i] = state.configuration_ [i];
return dofArray;
} catch (const std::exception& exc) {
@@ -840,6 +852,20 @@ namespace hpp {
}
}
+ CORBA::Short RbprmBuilder::generateStateInContact(const hpp::floatSeq& configuration,
+ const hpp::floatSeq& direction,const hpp::floatSeq& acceleration, const double robustnessThreshold ) throw (hpp::Error)
+ {
+ try
+ {
+ rbprm::State state = generateContacts_internal(configuration,direction,acceleration,robustnessThreshold);
+ lastStatesComputed_.push_back(state);
+ lastStatesComputedTime_.push_back(std::make_pair(-1., state));
+ return lastStatesComputed_.size()-1;
+ } catch (const std::exception& exc) {
+ throw hpp::Error (exc.what ());
+ }
+ }
+
hpp::floatSeq* RbprmBuilder::generateGroundContact(const hpp::Names_t& contactLimbs) throw (hpp::Error)
{
if(!fullBodyLoaded_)
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 836b073ff088fa3172424bfc949a4367de2f1f1f..253f951ea126af25a6337cec5d4b8b7c8f2fec9a 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -221,8 +221,12 @@ namespace hpp {
virtual double getSampleValue(const char* limb, const char* valueName, unsigned short sampleId) throw (hpp::Error);
virtual double getEffectorDistance(unsigned short state1, unsigned short state2) throw (hpp::Error);
+ rbprm::State generateContacts_internal(const hpp::floatSeq& configuration,
+ const hpp::floatSeq& direction,const hpp::floatSeq& acceleration, const double robustnessThreshold ) throw (hpp::Error);
virtual hpp::floatSeq* generateContacts(const hpp::floatSeq& configuration,
const hpp::floatSeq& direction, const hpp::floatSeq& acceleration, const double robustnessThreshold) throw (hpp::Error);
+ virtual CORBA::Short generateStateInContact(const hpp::floatSeq& configuration,
+ 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);