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

[contact-generation] add method generateStateInContact in fullBody

parent 86e0f125
......@@ -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
......
......@@ -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
......
......@@ -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_)
......
......@@ -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);
......
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