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

[contact-generation] add method to set start/end state from stateId

parent f91465fe
......@@ -280,11 +280,18 @@ module hpp
in double disableEffectorCollision,in double grasp) raises (Error);
/// Set the start state of a contact generation problem
/// environment, ordered by their efficiency
/// \param dofArray start configuration of the robot
/// \param contactLimbs ids of the limb in contact for the state
void setStartState(in floatSeq dofArray, in Names_t contactLimbs) raises (Error);
/// Set the start state of a contact generation problem
/// \param stateId : the id of the state, in fullBody
void setStartStateId(in unsigned short stateId) raises (Error);
/// Set the end state of a contact generation problem
/// \param stateId : the id of the state, in fullBody
void setEndStateId(in unsigned short stateId) raises (Error);
/// Compute effector contact points and normals for a given configuration
/// in local coordinates
/// \param dofArray configuration of the robot
......
......@@ -295,7 +295,7 @@ class FullBody (object):
def getSampleValue(self, limbName, valueName, sampleId):
return self.client.rbprm.rbprm.getSampleValue(limbName, valueName, sampleId)
## Initialize the first configuration of the path discretization
## Initialize the first configuration of the path interpolation
# with a balanced configuration for the interpolation problem;
#
# \param configuration the desired start configuration
......@@ -303,6 +303,17 @@ class FullBody (object):
def setStartState(self, configuration, contacts):
return self.client.rbprm.rbprm.setStartState(configuration, contacts)
## Initialize the first state of the path interpolation
# \param stateId the Id of the desired start state in fullBody
def setStartStateId(self,stateId):
return self.client.rbprm.rbprm.setStartStateId(stateId)
## Initialize the goal state of the path interpolation
# \param stateId the Id of the desired start state in fullBody
def setEndStateId(self,stateId):
return self.client.rbprm.rbprm.setEndStateId(stateId)
## Create a state given a configuration and contacts
#
# \param configuration the desired start configuration
......
......@@ -1286,6 +1286,41 @@ namespace hpp {
}
}
void RbprmBuilder::setStartStateId(unsigned short stateId) throw (hpp::Error){
try{
if(lastStatesComputed_.size() == 0)
{
throw std::runtime_error ("states not yet computed, call interpolate() first.");
}
if(lastStatesComputed_.size() <= stateId){
throw std::runtime_error ("invalid state id : "+std::string(""+stateId)+" number of state = "+std::string(""+lastStatesComputed_.size()));
}
startState_ = lastStatesComputed_[stateId];
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
void RbprmBuilder::setEndStateId(unsigned short stateId) throw (hpp::Error){
try{
if(lastStatesComputed_.size() == 0)
{
throw std::runtime_error ("states not yet computed, call interpolate() first.");
}
if(lastStatesComputed_.size() <= stateId){
throw std::runtime_error ("invalid state id : "+std::string(""+stateId)+" number of state = "+std::string(""+lastStatesComputed_.size()));
}
endState_ = lastStatesComputed_[stateId];
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
double RbprmBuilder::getTimeAtState(unsigned short stateId)throw (hpp::Error){
try
......
......@@ -252,6 +252,8 @@ namespace hpp {
virtual void setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual void setStartStateId(unsigned short stateId) throw (hpp::Error);
virtual void setEndStateId(unsigned short stateId) throw (hpp::Error);
virtual hpp::floatSeq* computeContactForConfig(const hpp::floatSeq& configuration, const char* limbNam) throw (hpp::Error);
virtual hpp::floatSeqSeq* computeContactPoints(unsigned short cId) throw (hpp::Error);
virtual hpp::floatSeqSeq* computeContactPointsAtState(unsigned short cId, unsigned short isIntermediate) 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