diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl index 591e2f475513481b8acd3f1313635d35709f0577..61c7ad8083df6962acb80bae7eb3708c5ca9c0bf 100755 --- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl +++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl @@ -699,6 +699,9 @@ module hpp boolean isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error); + boolean isDynamicallyReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error); + + }; // interface Robot }; // module rbprm }; // module corbaserver diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py index b9934d36a659c8d2e62a3a3a0f7e157807817196..b47c86794f0a467b941436731ae71471e87862fe 100755 --- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py +++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py @@ -1076,8 +1076,9 @@ class FullBody (object): def areKinematicsConstraintsVerifiedForState(self,stateFrom, point): return self.client.rbprm.rbprm.areKinematicsConstraintsVerifiedForState(stateFrom,point) - - def isReachableFromState(self,stateFrom,stateTo): return self.client.rbprm.rbprm.isReachableFromState(stateFrom,stateTo) + def isDynamicallyReachableFromState(self,stateFrom,stateTo): + return self.client.rbprm.rbprm.isDynamicallyReachableFromState(stateFrom,stateTo) + diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc index 64603eab8a2594c953895c58634cc555cc21b584..dce1c4d2b15f65c5dc5de304d7d4e2fa5776af20 100755 --- a/src/rbprmbuilder.impl.cc +++ b/src/rbprmbuilder.impl.cc @@ -3042,6 +3042,18 @@ assert(s2 == s1 +1); + bool RbprmBuilder::isDynamicallyReachableFromState(unsigned short stateFrom,unsigned short stateTo)throw (hpp::Error){ + if(!fullBodyLoaded_){ + throw std::runtime_error ("fullBody not loaded"); + } + if(stateTo >= lastStatesComputed_.size() || stateFrom >= lastStatesComputed_.size()){ + throw std::runtime_error ("Unexisting state ID"); + } + reachability::Result res = reachability::isReachableDynamic(fullBody(),lastStatesComputed_[stateFrom],lastStatesComputed_[stateTo]); + return (res.success()); + } + + } // namespace impl } // namespace rbprm } // namespace hpp diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh index 905ebe1907478cca7f5dbc5f668f7fe387b4ca06..3e5d750ddb6419c7e31af62bd029b96c0791b8b9 100755 --- a/src/rbprmbuilder.impl.hh +++ b/src/rbprmbuilder.impl.hh @@ -341,6 +341,7 @@ namespace hpp { virtual bool areKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error); virtual bool areKinematicsConstraintsVerifiedForState(unsigned short stateId,const hpp::floatSeq &point)throw (hpp::Error); virtual bool isReachableFromState(unsigned short stateFrom,unsigned short stateTo)throw (hpp::Error); + virtual bool isDynamicallyReachableFromState(unsigned short stateFrom,unsigned short stateTo)throw (hpp::Error); void selectFullBody (const char* name) throw (hpp::Error)