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

[python api] add state.projectToRoot method

parent b606a2c5
......@@ -154,6 +154,13 @@ module hpp
/// \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 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);
/// Project a state into a COM
/// \param stateId target state
......@@ -735,6 +735,14 @@ class FullBody (Robot):
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.
# \param state index of first state.
......@@ -141,6 +141,9 @@ class State (object):
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,, 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
def addNewContact(state, limbName, p, n, num_max_sample = 0):
sId =, limbName, p, n, num_max_sample)
def addNewContact(state, limbName, p, n, num_max_sample = 0, lockOtherJoints= False):
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.;
ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
CollisionValidationPtr_t val = fullBody()->GetCollisionValidation();
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