diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl index 6984cb781a8e6276fdeceb04203967f1f3e72fd6..7c31124a08f9505090877b2b10b25e6925a7c197 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 b20badf94318ae172bf7f0c64c4a5f15c3bd6c10..d3fe1e3a762c5bc5fcdc30a9613d148e34e15e49 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 0000000000000000000000000000000000000000..eab7645220a9bb27f8fee067d65204a90e897868 --- /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 1b0d534451d2cb348e15e69a8fb37ca1b5b5e621..49deed89e0aaa69cf407101d61949b799db349f4 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 27caedd3f2e8bff6c1b39ec54dd704593396548e..59b4cae3226584867030b217d0aee7eda66ea2d1 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 5021b175d1cd26ea5d98c7d44de1ef667b0ada45..c98d716c7ddd4b5479b544610a32403c94f6956b 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 d5fd072e751046be59718f050fce8af27badf47c..967b748683bc7637d91a9b34b3e0a44b7d261009 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);