Commit 2807b9ac authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[python api] add state.projectToRoot method

parent b606a2c5
......@@ -148,25 +148,32 @@ module hpp
/// [z_inf, z_sup, y_inf, y_sup, x_inf, x_sup]
void boundSO3(in floatSeq limitszyx) raises (Error);
/// Project a state into a COM
///
/// \param stateId target state
/// \param com target com
double projectStateToCOM(in unsigned short stateId, in floatSeq com, in unsigned short max_num_sample) raises (Error);
/// Project a state into a COM
///
/// \param stateId target state
/// \param com target com
double projectStateToCOM(in unsigned short stateId, in floatSeq com, in unsigned short max_num_sample) raises (Error);
/// Project a state into a COM
///
/// \param stateId target state
/// \param com target com
double setConfigAtState(in unsigned short stateId, in floatSeq config) raises (Error);
/// Project a state into a given root position and orientation
///
/// \param stateId target state
/// \param root the root configuration (size 7)
double projectStateToRoot(in unsigned short stateId, in floatSeq root) raises (Error);
/// Create a state and push it to the state array
///
/// \param q configuration
/// \param names list of effectors in contact
/// \return stateId
short createState(in floatSeq configuration, in Names_t contactLimbs) raises (Error);
/// Project a state into a COM
///
/// \param stateId target state
/// \param com target com
double setConfigAtState(in unsigned short stateId, in floatSeq config) raises (Error);
/// Create a state and push it to the state array
///
/// \param q configuration
/// \param names list of effectors in contact
/// \return stateId
short createState(in floatSeq configuration, in Names_t contactLimbs) raises (Error);
/// Get Sample configuration by its id
/// \param sampleName name of the limb from which to retrieve a sample
......
......@@ -734,6 +734,14 @@ class FullBody (Robot):
# \return whether the projection was successful
def projectStateToCOM(self, state, targetCom, maxNumSample = 0):
return self.clientRbprm.rbprm.projectStateToCOM(state, targetCom, maxNumSample) > 0
## Project a given state into a given root position
# and update the state configuration.
# \param state index of first state.
# \param root : root configuration (size 7)
# \return whether the projection was successful
def projectStateToRoot(self, state, root):
return self.clientRbprm.rbprm.projectStateToRoot(state, root) > 0
## Project a given state into a given COM position
# and update the state configuration.
......
......@@ -140,6 +140,9 @@ class State (object):
return self.fullBody.projectToCom(self.sId, targetCom)
else:
return self.fullBody.projectStateToCOM(self.sId, targetCom,maxNumSample) > 0
def projectToRoot(self,targetRoot):
return self.fullBody.projectStateToRoot(self.sId,targetRoot) > 0
def getComConstraint(self, limbsCOMConstraints, exceptList = []):
return get_com_constraint(self.fullBody, self.sId, self.cl.getConfigAtState(self.sId), limbsCOMConstraints, interm = self.isIntermediate, exceptList = exceptList)
......@@ -168,10 +171,13 @@ class StateHelper(object):
# \param limbName name of the considered limb to create contact with
# \param p 3d position of the point
# \param n 3d normal of the contact location center
# \param max_num_sample number of times it will try to randomly sample a configuration before failing
# \param lockOtherJoints : if True, the values of all the joints exepct the ones of 'limbName' are constrained to be constant.
# This only affect the first projection, if max_num_sample > 0 and the first projection was unsuccessfull, the parameter is ignored
# \return (State, success) whether the creation was successful, as well as the new state
@staticmethod
def addNewContact(state, limbName, p, n, num_max_sample = 0):
sId = state.cl.addNewContact(state.sId, limbName, p, n, num_max_sample)
def addNewContact(state, limbName, p, n, num_max_sample = 0, lockOtherJoints= False):
sId = state.cl.addNewContact(state.sId, limbName, p, n, num_max_sample,lockOtherJoints)
if(sId != -1):
return State(state.fullBody, sId = sId), True
return state, False
......
......@@ -851,6 +851,28 @@ namespace hpp {
return projectStateToCOMEigen(stateId, com_target, max_num_sample);
}
double RbprmBuilder::projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root) throw (hpp::Error)
{
pinocchio::Configuration_t root_target = dofArrayToConfig (7, root);
if(lastStatesComputed_.size() <= stateId)
{
throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
}
State s = lastStatesComputed_[stateId];
projection::ProjectionReport rep = projection::projectToRootConfiguration(fullBody(),root_target,s);
double success = 0.;
if(rep.success_){
ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
CollisionValidationPtr_t val = fullBody()->GetCollisionValidation();
if(val->validate(rep.result_.configuration_,rport)){
lastStatesComputed_[stateId] = rep.result_;
lastStatesComputedTime_[stateId].second = rep.result_;
success = 1.;
}
}
return success;
}
rbprm::State RbprmBuilder::generateContacts_internal(const hpp::floatSeq& configuration,
const hpp::floatSeq& direction,const hpp::floatSeq& acceleration, const double robustnessThreshold ) throw (hpp::Error)
......
......@@ -330,6 +330,7 @@ namespace hpp {
double projectStateToCOMEigen(unsigned short stateId, const pinocchio::Configuration_t& com_target, unsigned short maxNumeSamples)throw (hpp::Error);
virtual double projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error);
virtual double projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root) throw (hpp::Error);
virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
virtual void saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error);
virtual hpp::floatSeq* getOctreeBox(const char* limbName, double sampleId) throw (hpp::Error);
......
Markdown is supported
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