From a1882e78b81badd870899a6af63c209b4ca61585 Mon Sep 17 00:00:00 2001
From: t steve <pro@stevetonneau.fr>
Date: Tue, 2 May 2017 06:29:50 +0200
Subject: [PATCH] added method to test whether a state is balanced
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 4 +++
script/scenarios/demos/darpa_hyq.py | 6 ++--
script/scenarios/demos/rund.sh | 6 ++++
.../demos/stair_bauzil_hrp2_interp.py | 28 +++++++++++++------
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 8 ++++++
src/rbprmbuilder.impl.cc | 17 +++++++++++
src/rbprmbuilder.impl.hh | 1 +
7 files changed, 59 insertions(+), 11 deletions(-)
create mode 100755 script/scenarios/demos/rund.sh
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 6984cb7..7c31124 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -482,6 +482,10 @@ module hpp
/// \return size 7 position + quaternion of the octree root
floatSeq getOctreeTransform(in string limbname, in floatSeq dofArray) raises (Error);
+ /// returns whether a state is in static equilibrium
+ /// \param state id of the state
+ double isStateBalanced(in unsigned short state) 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
diff --git a/script/scenarios/demos/darpa_hyq.py b/script/scenarios/demos/darpa_hyq.py
index b20badf..d3fe1e3 100644
--- a/script/scenarios/demos/darpa_hyq.py
+++ b/script/scenarios/demos/darpa_hyq.py
@@ -183,11 +183,11 @@ def d(step=0.06):
print "computing contact plan"
genPlan(step)
-def e():
+def e(step = 0.5):
print "displaying contact plan"
- contactPlan()
+ contactPlan(step)
print "Root path generated in " + str(tp.t) + " ms."
#~ d();e()
-d(0.0005)
+d(0.004);e(0.01)
diff --git a/script/scenarios/demos/rund.sh b/script/scenarios/demos/rund.sh
new file mode 100755
index 0000000..eab7645
--- /dev/null
+++ b/script/scenarios/demos/rund.sh
@@ -0,0 +1,6 @@
+#!/bin/bash
+
+gepetto-viewer-server &
+ipython -i --no-confirm-exit ./$1
+
+pkill -f 'gepetto-viewer-server'
diff --git a/script/scenarios/demos/stair_bauzil_hrp2_interp.py b/script/scenarios/demos/stair_bauzil_hrp2_interp.py
index 1b0d534..49deed8 100755
--- a/script/scenarios/demos/stair_bauzil_hrp2_interp.py
+++ b/script/scenarios/demos/stair_bauzil_hrp2_interp.py
@@ -190,14 +190,14 @@ def genPlan(stepsize=0.1):
end = time.clock()
print "Contact plan generated in " + str(end-start) + "seconds"
-def contactPlan():
+def contactPlan(step = 0.5):
+ r.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
- r.client.gui.setVisibility("hrp2_14", "ON")
tp.r.client.gui.setVisibility("toto", "OFF")
- tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
- for i in range(0,len(configs)-1):
+ tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
+ for i in range(0,len(configs)):
r(configs[i]);
- time.sleep(0.5)
+ time.sleep(step)
def a():
@@ -216,11 +216,23 @@ def d(step=0.1):
print "computing contact plan"
genPlan(step)
-def e():
+def e(step = 0.5):
print "displaying contact plan"
- contactPlan()
+ contactPlan(step)
print "Root path generated in " + str(tp.t) + " ms."
-d(0.01); e()
+d(0.05); e(0.01)
+
+print "Root path generated in " + str(tp.t) + " ms."
+
+#~ from gen_data_from_rbprm import *
+#~
+#~ for config in configs:
+ #~ r(config)
+ #~ print(fullBody.client.basic.robot.getComPosition())
+#~
+
+#~ gen_and_save(fullBody,configs, "stair_bauzil_contacts_data")
+#~ main()
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 27caedd..59b4cae 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -596,6 +596,14 @@ class FullBody (object):
else:
return False
+ ## 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 isStateBalanced(self, stateId, robustness = 0):
+ return self.client.rbprm.rbprm.isStateBalanced(stateId) > robustness
+
## Updates limb databases with a user chosen computation
#
# \param analysis name of computation
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 5021b17..c98d716 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -2065,6 +2065,23 @@ assert(s2 == s1 +1);
}
}
+ double RbprmBuilder::isStateBalanced(unsigned short stateId) throw (hpp::Error)
+ {
+ try
+ {
+ if(lastStatesComputed_.size() <= stateId)
+ {
+ throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
+ }
+ return stability::IsStable(fullBody_,lastStatesComputed_[stateId]);
+ }
+ catch(std::runtime_error& e)
+ {
+ std::cout << "ERROR " << e.what() << std::endl;
+ throw Error(e.what());
+ }
+ }
+
void RbprmBuilder::runSampleAnalysis(const char* analysis, double isstatic) throw (hpp::Error)
{
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index d5fd072..967b748 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -208,6 +208,7 @@ namespace hpp {
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);
+ virtual double isStateBalanced(unsigned short stateId) throw (hpp::Error);
virtual void runSampleAnalysis(const char* analysis, double isstatic) throw (hpp::Error);
virtual hpp::floatSeq* runLimbSampleAnalysis(const char* limbname, const char* analysis, double isstatic) throw (hpp::Error);
virtual void dumpProfile(const char* logFile) throw (hpp::Error);
--
GitLab