diff --git a/python/MetaTask6d.py b/python/MetaTask6d.py deleted file mode 100644 index 44e55f58b2079162abac63c83c4408c5c12b7d18..0000000000000000000000000000000000000000 --- a/python/MetaTask6d.py +++ /dev/null @@ -1,20 +0,0 @@ -from dynamic_graph import plug -from dynamic_graph.sot.core import GainAdaptive -from dynamic_graph.sot.dyninv import TaskDynPD -from dynamic_graph.sot.core.meta_task_6d import MetaTask6d - -class MetaTaskDyn6d(MetaTask6d): - def createTask(self): - self.task = TaskDynPD('task'+self.name) - self.task.dt.value = 1e-3 - def createGain(self): - self.gain = GainAdaptive('gain'+self.name) - self.gain.set(1050,45,125e3) - def plugEverything(self): - self.feature.sdes.value = self.featureDes.name - plug(self.dyn.signal(self.opPoint),self.feature.signal('position')) - plug(self.dyn.signal('J'+self.opPoint),self.feature.signal('Jq')) - self.task.add(self.feature.name) - plug(self.dyn.velocity,self.task.qdot) - plug(self.task.error,self.gain.error) - plug(self.gain.gain,self.task.controlGain) diff --git a/python/dyndebug.py b/python/dyndebug.py index acb6d49b60fb17355274c23fdba84fac269ff1a8..86be708d55808c21a3ede269b05aa8f5eda4109d 100644 --- a/python/dyndebug.py +++ b/python/dyndebug.py @@ -86,16 +86,6 @@ def next(): inc() #runner.once() # --- shortcuts ------------------------------------------------- @optionalparentheses -def n(): - inc() - qdot() -@optionalparentheses -def n5(): - for loopIdx in range(5): inc() -@optionalparentheses -def n10(): - for loopIdx in range(10): inc() -@optionalparentheses def q(): if 'dyn' in globals(): print dyn.ffposition.__repr__() print robot.state.__repr__() @@ -167,6 +157,19 @@ plug(gCom.gain,taskCom.controlGain) # Current choice gCom.set(1050,45,125e3) +# ---- CONTACT ----------------------------------------- +# Left foot contact +contactLF = MetaTaskDyn6d('contact_lleg',dyn,'lf','left-ankle') +contactLF.support = ((0.11,-0.08,-0.08,0.11),(-0.045,-0.045,0.07,0.07),(-0.105,-0.105,-0.105,-0.105)) +contactLF.feature.frame('desired') +sot._LF_p.value = contactLF.support + +# Right foot contact +contactRF = MetaTaskDyn6d('contact_rleg',dyn,'rf','right-ankle') +contactRF.support = ((0.11,-0.08,-0.08,0.11),(-0.07,-0.07,0.045,0.045),(-0.105,-0.105,-0.105,-0.105)) +contactRF.feature.frame('desired') +sot._RF_p.value = contactRF.support + # --- SOT Dyn OpSpaceH -------------------------------------- # SOT controller. sot = SolverOpSpace('sot') @@ -179,31 +182,15 @@ plug(dyn.dynamicDrift,sot.dyndrift) plug(dyn.velocity,sot.velocity) plug(sot.control,robot.control) - # For the integration of q = int(int(qddot)). plug(sot.acceleration,robot.acceleration) -# ---- CONTACT ----------------------------------------- -# Contact definition +# --- RUN ------------------------------------------------ -supportLF = ((0.11,-0.08,-0.08,0.11),(-0.045,-0.045,0.07,0.07),(-0.105,-0.105,-0.105,-0.105)) -supportRF = ((0.11,-0.08,-0.08,0.11),(-0.07,-0.07,0.045,0.045),(-0.105,-0.105,-0.105,-0.105)) +featureComDes.errorIN.value = (0.06,0,0.8) -# Left foot contact -contactLF = MetaTaskDyn6d('contact_lleg',dyn,'lf','left-ankle') -contactLF.feature.frame('desired') sot.addContactFromTask(contactLF.task.name,'LF') -sot._LF_p.value = supportLF - -# Right foot contact -contactRF = MetaTaskDyn6d('contact_rleg',dyn,'rf','right-ankle') -contactRF.feature.frame('desired') sot.addContactFromTask(contactRF.task.name,'RF') -sot._RF_p.value = supportRF - -# --- RUN ------------------------------------------------ - -featureComDes.errorIN.value = (0.06,0,0.8) sot.push('taskCom') #go