From 415080e269ba9463cf7ca2b2811a8c19e26d3377 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Fri, 15 Apr 2011 15:56:20 +0200
Subject: [PATCH] Modify meta-task to account for the new version in sot-core.
---
python/MetaTask6d.py | 66 +--------------
python/dyndebug.py | 195 ++++++++++++++++++-------------------------
2 files changed, 87 insertions(+), 174 deletions(-)
diff --git a/python/MetaTask6d.py b/python/MetaTask6d.py
index b2b2d8f..44e55f5 100644
--- a/python/MetaTask6d.py
+++ b/python/MetaTask6d.py
@@ -1,56 +1,15 @@
from dynamic_graph import plug
-from dynamic_graph.sot.core import *
-from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
-from dynamic_graph.sot.dynamics import *
-from dynamic_graph.sot.dyninv import *
-
-def toFlags(arr):
- l=max(arr)+1
- lres=[0]*l
- for i in arr:
- lres[i]=1
- lres.reverse()
- res=''
- for i in lres:
- res+=str(i)
- return res
-
-class MetaTask6d(object):
- name=''
- opPoint=''
- dyn=0
- derivator=0
- task=0
- feature=0
- featureDes=0
-
- def opPointExist(self,opPoint):
- sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint, self.dyn.signals())
- sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J'+opPoint, self.dyn.signals())
- return len(sigsP)==1 & len(sigsJ)==1
-
- def defineDynEntities(self,dyn):
- self.dyn=dyn
-
- def createOpPoint(self,opPoint,opPointRef = 'right-wrist'):
- self.opPoint=opPoint
- if self.opPointExist(opPoint): return
- self.dyn.createOpPoint(opPoint,opPointRef)
-
- def createFeatures(self):
- self.feature = FeaturePoint6d('feature'+self.name)
- self.featureDes = FeaturePoint6d('feature'+self.name+'_ref')
- self.feature.selec.value = '111111'
- self.feature.frame('current')
+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'))
@@ -59,20 +18,3 @@ class MetaTask6d(object):
plug(self.dyn.velocity,self.task.qdot)
plug(self.task.error,self.gain.error)
plug(self.gain.gain,self.task.controlGain)
-
- def __init__(self,name,dyn,opPoint,opPointRef='right-wrist'):
- self.name=name
- self.defineDynEntities(dyn)
- self.createOpPoint(opPoint,opPointRef)
- self.createFeatures()
- self.createTask()
- self.createGain()
- self.plugEverything()
-
- @property
- def ref(self):
- return self.featureDes.position.value
-
- @ref.setter
- def ref(self,m):
- self.featureDes.position.value = m
diff --git a/python/dyndebug.py b/python/dyndebug.py
index 18fa661..ebe1095 100644
--- a/python/dyndebug.py
+++ b/python/dyndebug.py
@@ -6,56 +6,71 @@ from dynamic_graph.sot.dyninv import *
import dynamic_graph.script_shortcuts
from dynamic_graph.script_shortcuts import optionalparentheses
from dynamic_graph.matlab import matlab
-from MetaTask6d import MetaTask6d
+from MetaTask6d import MetaTaskDyn6d
-# --- Dynamic parameters ---
-hrp2_14_pkgdatarootdir = "/home/nmansard/compil/devgiri/hpp2/share/hrp2_14"
-modelDir = hrp2_14_pkgdatarootdir
-xmlDir = hrp2_14_pkgdatarootdir
-modelName = 'HRP2JRLmainsmall.wrl'
-specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
-jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
-robotDim=36
+from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
+robotName = 'hrp10small'
+
+from numpy import *
+def totuple( a ):
+ al=a.tolist()
+ res=[]
+ for i in range(a.shape[0]):
+ res.append( tuple(al[i]) )
+ return tuple(res)
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
-robot = RobotDynSimu("robot")
+robotDim=robotDimension[robotName]
+RobotClass = RobotDynSimu
+robot = RobotClass("robot")
robot.resize(robotDim)
-#robot.set( (0,0,0,0,0,0, -0.0091163033083694940822,-0.091409963793829082657,-0.47197874328281280709,0.84038019261713603481,-0.47023279905304871118,0.089662407859063653071,0.0095078180225693850747,0.091110286804129178573,-0.4694503518480188653,0.83530799538565336793,-0.46768619090392338222,-0.093802946636709280681,-8.7564658296357018321e-05,0.0032643187737393364843,-7.8338082008638037324e-06,0.00019474257801330629915,0.25837025731361307201,-0.17509910214200960499,-6.1173425555032122825e-05,-0.52495354876847799552,3.1825099712999219606e-06,-0.00025760004742291479777,-3.4121048192112107608e-06,0.25836727162795458668,0.17432209754621175168,-8.8902395499734237117e-05,-0.52498369084585783106,-3.4610294148795152394e-07,-0.00026540143977199129972,1.004984814529256132e-06) )
-robot.set( ( 0.0274106623863, 0.143843868989, 0.646921914726, 0.00221064938462, 0.101393756965, 1.36729741242e-05, -0.00911630330837, -0.0914099637938, -0.471978743283, 0.840380192617, -0.470232799053, 0.0896624078591, 0.00950781802257, 0.0911102868041, -0.469450351848, 0.835307995386, -0.467686190904, -0.0938029466367, -8.75646582964e-05, 0.00326431877374, -7.83380820086e-06, 0.000194742578013, 0.258370257314, -0.175099102142, -6.1173425555e-05, -0.524953548768, 3.1825099713e-06, -0.000257600047423, -3.41210481921e-06, 0.258367271628, 0.174322097546, -8.89023954997e-05, -0.524983690846, -3.46102941488e-07, -0.000265401439772, 1.00498481453e-06 ) )
-dt=1e-3
+robot.set( initialConfig[robotName] )
+dt=5e-3
# --- VIEWER -------------------------------------------------------------------
# --- VIEWER -------------------------------------------------------------------
# --- VIEWER -------------------------------------------------------------------
-try:
- import robotviewer
-
- def stateFullSize(robot):
- simulation_angles = [float(val) for val in robot.state.value]
- simulation_angles += 10*[0.0]
- return simulation_angles
- RobotDynSimu.stateFullSize = stateFullSize
-
- robot.viewer = robotviewer.client('XML-RPC')
- robot.viewer.updateElementConfig('hrp',robot.stateFullSize())
-
- def incrementView( robot,dt ):
- robot.incrementNoView(dt)
- robot.viewer.updateElementConfig('hrp',robot.stateFullSize())
- RobotDynSimu.incrementNoView = RobotDynSimu.increment
- RobotDynSimu.increment = incrementView
+if 1:#try:
+ import robotviewer
+
+ def stateFullSize(robot):
+ return [float(val) for val in robot.state.value]+10*[0.0]
+ RobotClass.stateFullSize = stateFullSize
+ robot.viewer = robotviewer.client('XML-RPC')
+ # Check the connection
+ robot.viewer.updateElementConfig('hrp',robot.stateFullSize())
+
+ def refreshView( robot ):
+ robot.viewer.updateElementConfig('hrp',robot.stateFullSize())
+ RobotClass.refresh = refreshView
+ def incrementView( robot,dt ):
+ robot.incrementNoView(dt)
+ robot.refresh()
+ RobotClass.incrementNoView = RobotClass.increment
+ RobotClass.increment = incrementView
+ def setView( robot,*args ):
+ robot.setNoView(*args)
+ robot.refresh()
+ RobotClass.setNoView = RobotClass.set
+ RobotClass.set = setView
+
+ robot.refresh()
+try:pass
except:
print "No robot viewer, sorry."
robot.viewer = None
# --- MAIN LOOP ------------------------------------------
+
+qs=[]
def inc():
robot.increment(dt)
- tr.triger.recompute( robot.control.time )
+ qs.append(robot.state.value)
+
from ThreadInterruptibleLoop import *
@loopInThread
def loop():
@@ -67,18 +82,44 @@ def go(): runner.play()
@optionalparentheses
def stop(): runner.pause()
@optionalparentheses
-def next(): runner.once()
+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__()
+@optionalparentheses
+def qdot(): print robot.control.__repr__()
+@optionalparentheses
+def t(): print robot.state.time-1
+@optionalparentheses
+def iter(): print 'iter = ',robot.state.time
+@optionalparentheses
+def status(): print runner.isPlay
# --- DYN ----------------------------------------------------------------------
-# --- DYN ----------------------------------------------------------------------
-# --- DYN ----------------------------------------------------------------------
+modelDir = pkgDataRootDir[robotName]
+xmlDir = pkgDataRootDir[robotName]
+specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
+jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
dyn = Dynamic("dyn")
-dyn.setFiles(modelDir, modelName,specificitiesPath,jointRankPath)
+dyn.setFiles(modelDir,modelName[robotName],specificitiesPath,jointRankPath)
dyn.parse()
-dyn.inertiaRotor.value = (0,0,0,0,0,0,1.01e-4,6.96e-4,1.34e-4,1.34e-4,6.96e-4,6.96e-4,1.01e-4,6.96e-4,1.34e-4,1.34e-4,6.96e-4,6.96e-4,6.96e-4,6.96e-4,1.10e-4,1.10e-4,6.96e-4,6.60e-4,1.00e-4,6.60e-4,1.10e-4,1.00e-4,1.00e-4,6.96e-4,6.60e-4,1.00e-4,6.60e-4,1.10e-4,1.00e-4,1.00e-4)
-dyn.gearRatio.value = (0,0,0,0,0,0,384.0,240.0,180.0,200.0,180.0,100.0,384.0,240.0,180.0,200.0,180.0,100.0,207.69,381.54,100.0,100.0,219.23,231.25,266.67,250.0,145.45,350.0,200.0,219.23,231.25,266.67,250.0,145.45,350.0,200.0)
+dyn.inertiaRotor.value = inertiaRotor[robotName]
+dyn.gearRatio.value = gearRatio[robotName]
plug(robot.state,dyn.position)
plug(robot.velocity,dyn.velocity)
@@ -93,47 +134,13 @@ dyn.setProperty('ComputeAccelerationCoM','true')
robot.control.unplug()
-# --- CONTACT MODIFICATIONS --- ---------------------------------------------
-# Put the robot on the ground
-# These computations are done only for debug, to be use by the programmer
-# to compute (e.g. with matlab) the root position at start.
-
-sole = OpPointModifier('sole')
-dyn.createOpPoint('rf','right-ankle')
-dyn.createPosition('root','waist')
-
-plug(dyn.rf,sole.positionIN)
-sole.setTransformation(((1,0,0,0),(0,1,0,0),(0,0,1,-0.105),(0,0,0,1)))
-
-# 45deg
-# In AF: next; dyn.root ; sole.position ... copy/paste from AF to matlab
-# In Matlab:
-# Mref=[ s 0 s 0 ; 0 1 0 0 ; -s 0 s 0 ; 0 0 0 1]
-# Mref*inv(position)*root
-# robot.root [4,4]((0.99772,0.06735,0.00309,0.20444),(-0.06477,0.97027,-0.23318,0.06157),(-0.01870,0.23245,0.97243,0.39644),(0.00000,0.00000,0.00000,1.00000))
-
-# 10deg
-# a=10/180*pi; c=cos(a); s=sin(a); Mref=[ c 0 s 0 ; 0 1 0 0 ; -s 0 c 0 ; 0 0 0 1];
-# robot.root [4,4]((0.99841,0.03015,-0.04757,0.05129),(-0.04181,0.96266,-0.26745,0.02754),(0.03773,0.26901,0.96240,0.47705),(0.00000,0.00000,0.00000,1.00000))
-
-# 0deg robot.root [4,4]((0.99500,0.09983,-0.00000,0.01710),(-0.09977,0.99441,0.03462,-0.09466),(0.00346,-0.03445,0.99940,0.60432),(0.00000,0.00000,0.00000,1.00000))
-
-# two feet
-robot.setRoot(((0.994864055284,0.000210089058006,0.101219896104,0.0274106623863),(1.36027504855e-05,0.999997559482,-0.00220926360724,0.143843868989),(-0.101220113217,0.00219929382048,0.994861624442, 0.646921914726),(0,0,0,1)))
-
-# 0deg
-# robot.root [4,4]((0.994862921513, 0.000210047741424, 0.101231039102, 0.0274148975459),(1.3690802568e-05, 0.999997559008, -0.00220947747375, 0.143849747317),(-0.101231256093, 0.00219951314872, 0.994860490185, 0.646921274968),(0, 0, 0, 1))
-# 10deg
-# robot.root [4,4]((0.962169712502,0.000588801121243,0.272450174631,0.13933634465),(1.369076794e-05,0.999997559008,-0.00220947748355,0.143849747317),(-0.272450810525,0.00212962236724,0.962167355792,0.632333943842),(0,0,0,1))
-
# --- Task Dyn -----------------------------------------
-
# Task right hand
-task=MetaTask6d('rh',dyn,'task')
+task=MetaTaskDyn6d('rh',dyn,'task')
task.ref = ((0,0,-1,0.22),(0,1,0,-0.37),(1,0,0,.74),(0,0,0,1))
# Task LFoot: Move the right foot up.
-taskLF=MetaTask6d('lf',dyn,'lf','left-ankle')
+taskLF=MetaTaskDyn6d('lf',dyn,'lf','left-ankle')
taskLF.ref = ((1,0,0,0.0),(0,1,0,+0.29),(0,0,1,.15),(0,0,0,1))
# --- TASK COM ------------------------------------------------------
@@ -163,7 +170,7 @@ gCom.set(1050,45,125e3)
# --- SOT Dyn OpSpaceH --------------------------------------
# SOT controller.
sot = SolverOpSpace('sot')
-sot.setSize(30)
+sot.setSize(robotDim-6)
#sot.damping.value = 2e-2
sot.breakFactor.value = 10
@@ -181,55 +188,19 @@ plug(sot.acceleration,robot.acceleration)
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))
-# supportRF = ((0.12,-0.09,-0.09,0.12),(-0.08,-0.08,0.05,0.05),(-0.105,-0.105,-0.105,-0.105))
# Left foot contact
-contactLF = MetaTask6d('contact_lleg',dyn,'lf','left-ankle')
+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 = MetaTask6d('contact_rleg',dyn,'rf','right-ankle')
+contactRF = MetaTaskDyn6d('contact_rleg',dyn,'rf','right-ankle')
contactRF.feature.frame('desired')
sot.addContactFromTask(contactRF.task.name,'RF')
sot._RF_p.value = supportRF
-# constraint order
-# 19 16
-# ^ Front +X +---+
-# | 9 6 | R |
-# +--> Right -Y +---+ | |
-# | | +---+
-# ^X | L | 18 17(-8,-45)
-# Y | +---+ (-8,7)
-# <--+ 8 7(-8,-7)
-# (-8,45)
-
-# --- TRACE ----------------------------------------------
-
-from dynamic_graph.tracer import *
-from dynamic_graph.tracer_real_time import *
-tr = TracerRealTime('tr')
-tr.setBufferSize(10485760)
-tr.open('/tmp/','dyn_','.dat')
-tr.add('dyn.position','')
-tr.start()
-
-#robot.periodicCall addSignal tr.triger
-
-#tr.add('p6.error','position')
-tr.add('featureCom.error','comerror')
-tr.add('dyn.com','com')
-tr.add('sot.zmp','')
-#tr.add('sot.qdot','')
-tr.add('dyn.position','state')
-tr.add('dyn.ffposition','ffposition')
-# tr.add('gCom.gain','')
-# tr.add('gCom.error','gerror')
-
-tr.add('sot.control','')
-
# --- RUN ------------------------------------------------
featureComDes.errorIN.value = (0.06,0,0.8)
--
GitLab