Commit 9dc877aa authored by Steve Tonneau's avatar Steve Tonneau
Browse files

can assert blance of a configuration given contacts in pyhton

parent 4539d848
......@@ -206,6 +206,13 @@ module hpp
/// \return size 7 position + quaternion of the octree root
floatSeq getOctreeTransform(in string limbname, in floatSeq dofArray) raises (Error);
/// returns octree transform for a given robot configuration
/// \param config configuration tested on the robot
/// \param contacts name of the limbs in contact
/// \param robustnessTreshold robustness treshold used
/// \return whether the configuration is quasi-statically balanced
short isConfigBalanced(in floatSeq config, in Names_t contacts, in double robustnessTreshold) raises (Error);
}; // interface Robot
}; // module rbprm
}; // module corbaserver
......
......@@ -186,6 +186,17 @@ class FullBody (object):
def interpolateConfigs(self, configs):
return self.client.rbprm.rbprm.interpolateConfigs(configs)
## Given start and goal states
# generate a contact sequence over a list of configurations
#
# \param stepSize discretization step
# \param pathId Id of the path to compute from
def isConfigBalanced(self, config, names, robustness = 0):
if (self.client.rbprm.rbprm.isConfigBalanced(config, names, robustness) == 1):
return True
else:
return False
## Create octree nodes representation for a given limb
#
# \param stepSize discretization step
......
......@@ -624,6 +624,43 @@ namespace hpp {
}
}
CORBA::Short RbprmBuilder::isConfigBalanced(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs, double robustnessTreshold) throw (hpp::Error)
{
try{
rbprm::State testedState;
model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
model::Configuration_t save = fullBody_->device_->currentConfiguration();
std::vector<std::string> names = stringConversion(contactLimbs);
for(std::vector<std::string>::const_iterator cit = names.begin(); cit != names.end();++cit)
{
std::cout << "name " << * cit << std::endl;
const hpp::rbprm::RbPrmLimbPtr_t limb =fullBody_->GetLimbs().at(std::string(*cit));
testedState.contacts_[*cit] = true;
testedState.contactPositions_[*cit] = limb->effector_->currentTransformation().getTranslation();
testedState.contactRotation_[*cit] = limb->effector_->currentTransformation().getRotation();
// normal given by effector normal
const fcl::Vec3f normal = limb->effector_->currentTransformation().getRotation() * limb->normal_;
testedState.contactNormals_[*cit] = normal;
testedState.configuration_ = config;
++testedState.nbContacts;
}
fullBody_->device_->currentConfiguration(save);
fullBody_->device_->computeForwardKinematics();
if (stability::IsStable(fullBody_, testedState) >= robustnessTreshold)
{
return (CORBA::Short)(1);
}
else
{
return (CORBA::Short)(0);
}
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
void RbprmBuilder::SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver)
{
problemSolver_ = problemSolver;
......
......@@ -120,6 +120,7 @@ namespace hpp {
virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
virtual hpp::floatSeqSeq* GetOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
virtual hpp::floatSeq* getOctreeTransform(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
virtual CORBA::Short isConfigBalanced(const hpp::floatSeq& config, const hpp::Names_t& contactLimbs, double robustnessTreshold) throw (hpp::Error);
public:
void SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver);
......
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