From 24cc85a80ae890daf9085532b1d9d0dcc4ecd208 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Thu, 1 Feb 2018 15:37:09 +0100
Subject: [PATCH] [transition test] add python method 'isDynamicallyReachable'
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 3 +++
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 5 +++--
src/rbprmbuilder.impl.cc | 12 ++++++++++++
src/rbprmbuilder.impl.hh | 1 +
4 files changed, 19 insertions(+), 2 deletions(-)
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 591e2f4..61c7ad8 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 b9934d3..b47c867 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 64603ea..dce1c4d 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 905ebe1..3e5d750 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)
--
GitLab