From 0c21be3221171d2cf01a6bb0d159e16eae87110c Mon Sep 17 00:00:00 2001 From: Mansard <nmansard@laas.fr> Date: Thu, 28 Jul 2011 14:13:20 +0200 Subject: [PATCH] IVIGIT. --- CMakeLists.txt | 2 + python/dyndebug.py | 1 + python/kinedebug.py | 195 +++------------- python/kinesimple.py | 283 +++++++++++++++++++++++ src/dynamic_graph/sot/dyninv/__init__.py | 2 + 5 files changed, 317 insertions(+), 166 deletions(-) create mode 100644 python/kinesimple.py diff --git a/CMakeLists.txt b/CMakeLists.txt index f70429e..bd21a2e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,6 +55,7 @@ SET(libs task-dyn-pd task-dyn-joint-limits solver-op-space + solver-dyn-reduced solver-kine task-joint-limits @@ -82,6 +83,7 @@ SET(headers task-dyn-pd.h task-dyn-joint-limits.h solver-op-space.h + solver-dyn-reduced.h task-joint-limits.h task-inequality.h diff --git a/python/dyndebug.py b/python/dyndebug.py index d8152f3..2f2d179 100644 --- a/python/dyndebug.py +++ b/python/dyndebug.py @@ -10,6 +10,7 @@ from meta_task_dyn_6d import MetaTaskDyn6d from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor robotName = 'hrp10small' +robotName = 'hrp14small' from numpy import * def totuple( a ): diff --git a/python/kinedebug.py b/python/kinedebug.py index e9e9d1b..8615a48 100644 --- a/python/kinedebug.py +++ b/python/kinedebug.py @@ -43,7 +43,7 @@ try: RobotSimu.stateFullSize = stateFullSize robot.viewer = robotviewer.client('XML-RPC') -# robot.viewer.updateElementConfig('hrp',robot.stateFullSize()) + robot.viewer.updateElementConfig('hrp',robot.stateFullSize()) def refreshView( robot ): robot.viewer.updateElementConfig('hrp',robot.stateFullSize()) @@ -137,13 +137,13 @@ class MetaTaskKine6d( MetaTask6d ): # Task right hand taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist') -taskRH.ref = ((0,0,-1,0.22),(0,1,0,-0.37),(1,0,0,.74),(0,0,0,1)) taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist') -#TODO taskLH.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. taskRF=MetaTaskKine6d('rf',dyn,'rf','right-ankle') taskLF=MetaTaskKine6d('lf',dyn,'lf','left-ankle') +taskRF.task.controlGain.value = 10 #.5/dt +taskLF.task.controlGain.value = 10 #.5/dt # --- TASK COM ------------------------------------------------------ dyn.setProperty('ComputeCoM','true') @@ -161,7 +161,7 @@ taskCom.add('featureCom') gCom = GainAdaptive('gCom') plug(taskCom.error,gCom.error) plug(gCom.gain,taskCom.controlGain) -gCom.set(1,1,1) +gCom.setConstant(1) # --- TASK SUPPORT -------------------------------------------------- featureSupport = FeatureGeneric('featureSupport') @@ -206,11 +206,11 @@ plug(dyn.upperJl,taskJL.referenceSup) taskJL.dt.value = dt taskJL.selec.value = toFlags(range(6,robotDim)) -# --- SOT Dyn OpSpaceH -------------------------------------- +# --- SOT KINE OpSpaceH ------------------------------------------- # SOT controller. sot = SolverKine('sot') sot.setSize(robotDim) -#sot.damping.value = 2e-2 +sot.damping.value = 1e-6 plug(sot.control,robot.control) @@ -227,7 +227,7 @@ from dynamic_graph.tracer import * from dynamic_graph.tracer_real_time import * tr = TracerRealTime('tr') tr.setBufferSize(10485760) -tr.open('/tmp/','dyn_','.dat') +tr.open('/tmp/','','.dat') tr.start() #robot.periodicCall addSignal tr.triger @@ -240,11 +240,14 @@ tr.add('dyn.position','state') # tr.add('gCom.error','gerror') tr.add('sot.control','') +tr.add('taskJL.'+taskJL.normalizedPosition.name,'') +robot.after.addSignal('taskJL.'+taskJL.normalizedPosition.name) # --- shortcuts ------------------------------------------------- qn=taskJL.normalizedPosition q=taskJL.position comref=featureComDes.errorIN +com=featureCom.errorIN @optionalparentheses def iter(): print 'iter = ',robot.state.time @@ -253,19 +256,23 @@ def dump(): tr.dump() @optionalparentheses def status(): print runner.isPlay -# --- A FIRST MOTION --------------------------------------------- +matlab.prec=4 +#matlab.fullPrec=0 + +# --- RUN ------------------------------------------------ -if 0: - sot.addContact(taskLF) - sot.addContact(taskRF) - sot.push(taskCom.name) - sot.push(taskRH.task.name) +gCom.setByPoint(10,1,.1,.8) +#comref.value = ( 0.05,0.16,0.7 ) +comref.value = ( -0.02,-0.02,0.8 ) - taskRH.ref = ((0,0,-1,0.22),(0,1,0,-0.5),(1,0,0,1.24),(0,0,0,1)) - taskRH.gain.setConstant(1) - comref.value=( 0.059949815729, 0.2098857010921, 0.750396505072 ) +#taskRH.feature.selec.value = '111' +taskRH.gain.setConstant(1) + +# Hit the Y-border border of the COM +#taskRH.ref = ((0,0,-1,0.22),(0,1,0,-0.64),(1,0,0,1.24),(0,0,0,1)) +# Hit both X and Y borders of the COM +taskRH.ref = ((0,0,-1,0.25),(0,1,0,-0.64),(1,0,0,1.24),(0,0,0,1)) -# --- RUN ------------------------------------------------ #sot.damping.value=.1 sot.addContact(taskLF) @@ -274,158 +281,14 @@ sot.addContact(taskRF) sot.push(taskJL.name) sot.push(taskSupport.name) sot.push(taskRH.task.name) +#sot.push(taskCom.name) -taskRH.ref = ((0,0,-1,0.22),(0,1,0,-0.5),(1,0,0,1.24),(0,0,0,1)) -taskRH.gain.setConstant(1) -comref.value=( 0.059949815729, 0.2098857010921, 0.750396505072 ) - -tr.add('taskJL.normalizedPosition','qn') -robot.after.addSignal('taskJL.normalizedPosition') -robot.after.addSignal('taskJL.task') - -# impossible position (ok with damping): -taskRH.ref = ((0,0,-1,0.32),(0,1,0,-0.75),(1,0,0,1.24),(0,0,0,1)) -# feasible -taskRH.ref = ((0,0,-1,0.32),(0,1,0,-0.63),(1,0,0,1.24),(0,0,0,1)) - -# --- UMBRELLA ---------------------------------------------------- -# --- UMBRELLA ---------------------------------------------------- -# --- UMBRELLA ---------------------------------------------------- -import copy - -# M is the position of the top of the umbrella. -M=identity(4) -M[0][3]=.595 # Stick lenght -M[0][3]=.45 # without unbrella lenght -M[2][3]=-.19 # Lenght of the wrist -widthUmbrella=.41 # Width of the umbrella -# List of the polygon defining the umbrella hat. -umbrellaPolygon = [ [ +1,+1 ],[+1,-1],[-1,-1],[-1,+1] ] -umbrellaPointNames = [ 'a','b','c','d' ] - -# Creation of the Op Point modifiers for each vertex of the polygon. -dyn.createJacobian('Jrh0','right-wrist') -umbrellaModif = dict() -for i in range(len(umbrellaPolygon)): - point=umbrellaPolygon[i] - name=umbrellaPointNames[i] - modif=OpPointModifier('modif'+name) - modif.setEndEffector(False) - plug(dyn.rh,modif.positionIN) - plug(dyn.Jrh0,modif.jacobianIN) - Mi=copy.copy(M) - Mi[1][3]+=widthUmbrella*point[0] - Mi[2][3]+=widthUmbrella*point[1] - modif.setTransformation(totuple(Mi)) - umbrellaModif[name]=modif - -# Creation of the features for the edges of the polygon. -umbrellaFeature=dict() -for i in range(len(umbrellaPointNames)): - nameA=umbrellaPointNames[i] - if i+1<len(umbrellaPointNames): - nameB=umbrellaPointNames[i+1] - else: - nameB=umbrellaPointNames[0] - A=umbrellaModif[nameA] - B=umbrellaModif[nameB] - feature=FeatureProjectedLine('feature'+nameA+nameB) - plug(A.position,feature.xa) - plug(A.jacobian,feature.Ja) - plug(B.position,feature.xb) - plug(B.jacobian,feature.Jb) - feature.xc.value = (0,0) - umbrellaFeature[nameA+nameB]=feature - -# Adding everything to the tracer -for name in umbrellaPointNames: - tr.add(umbrellaModif[name].name+'.position',name) - #robot.after.addSignal(umbrellaModif[name].name+'.position') -for name,feature in umbrellaFeature.items(): - tr.add(feature.name+'.error','e'+name) - #robot.after.addSignal(feature.name+'.error') - - -A=umbrellaModif['a'].position -B=umbrellaModif['b'].position -C=umbrellaModif['c'].position -D=umbrellaModif['d'].position - -AB=umbrellaFeature['ab'] -BC=umbrellaFeature['bc'] -CD=umbrellaFeature['cd'] -DA=umbrellaFeature['da'] - -# Position at starting point: error is null -CD.xc.value=(-0.001012500000,-0.609998000000) -DA.xc.value=CD.xc.value -AB.xc.value=(0.818988000000,0.210001000000) -BC.xc.value=AB.xc.value - -CD.xc.value=(0.0,-0.6) -DA.xc.value=CD.xc.value -AB.xc.value=(0.8,0.2) -BC.xc.value=AB.xc.value - -robot.set((0.15787508826861599, 0.045511651751362708, 0.58971500036966162, -0.0040107700215134432, -0.45564238401733737, -0.52686538178073516, 0.44716913166621519, 0.30513254004984031, 0.080434789095732442, 1.1705171307901581, -0.84322874602000764, -0.085337729992559314, 0.34519179463400057, 0.51096319874334073, 0.39729314896741563, 0.79637015282207479, -0.74524853791029155, -0.3113477017777998, 0.59528786833733205, -0.0060236125952612008, 0.042089394399565112, 0.028985249939047025, -0.37671405575009481, -0.29002038141976838, -0.43761993851863368, -2.0077185039386283, -0.50346194284379353, 1.3345805041453564, 0.030948677237969232, 0.13701711567791786, 0.43501758221426229, 0.11316541844882388, -0.17894103295334104, 0.00080661744280727005, 0.054276071429870718, 0.0050874263605387498)) - -#AB.xc.value = (0,0,0) -#BC.xc.value = (0,0,1.2) -#CD.xc.value = (0,0,1.2) -#DA.xc.value = (0,0,1.2) - -taskAB=Task('taskAB') -taskAB.add(umbrellaFeature['ab'].name) -#taskAB.add(umbrellaFeature['bc'].name) -#taskAB.add(umbrellaFeature['cd'].name) -#taskAB.add(umbrellaFeature['da'].name) -taskAB.controlGain.value = 10 - -taskBC=Task('taskBC') -taskBC.add(umbrellaFeature['bc'].name) -taskBC.controlGain.value = 10 - -taskCD=Task('taskCD') -taskCD.add(umbrellaFeature['cd'].name) -taskCD.controlGain.value = 10 - - -# And finally, creating the task. -taskUmbrella=TaskInequality('taskUmbrella') -taskUmbrella.controlGain.value = .01 -taskUmbrella.referenceSup.value = (0,0,0,0) -taskUmbrella.dt.value=dt -for name,feature in umbrellaFeature.items(): - taskUmbrella.add(feature.name) - feature.xc.value = (0.5,-0.7) -tr.add('taskUmbrella.error','error') - - -sot.clear() -sot.push(taskUmbrella.name) - - -@attime(100) -def m1(): - "adding COM" - comref.value = ( 0.05,0.16,0.7 ) - sot.addContact(taskLF) - sot.addContact(taskRF) - sot.push(taskCom.name) - gCom.setByPoint(10,1,.1,.8) - -@attime(200) -def m2(): - "adding RH" - taskRH.feature.selec.value = '111' - X=mat(identity(4)) - X[0:3,3]=mat( (.3,-.6,1.3 )).H - taskRH.ref = totuple(X) - sot.push(taskRH.task.name) +#@attime(200) +def freeComZ(): + 'Free the Z component of the COM' + featureCom.selec.value = '11' attime(1000,stop,'pause') attime(1000,dump,'dump') -matlab.prec=4 -#matlab.fullPrec=0 diff --git a/python/kinesimple.py b/python/kinesimple.py new file mode 100644 index 0000000..40f88af --- /dev/null +++ b/python/kinesimple.py @@ -0,0 +1,283 @@ +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 * +import dynamic_graph.script_shortcuts +from dynamic_graph.script_shortcuts import optionalparentheses +from dynamic_graph.matlab import matlab +from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags +from attime import attime + +from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor +robotName = 'hrp14small' + +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 --------------------------------------------------------------- + +robotDim=robotDimension[robotName] +robot = RobotSimu("robot") +robot.resize(robotDim) + +robot.set( initialConfig[robotName] ) +dt=5e-3 + +# --- VIEWER ------------------------------------------------------------------- +# --- VIEWER ------------------------------------------------------------------- +# --- VIEWER ------------------------------------------------------------------- +try: + import robotviewer + + def stateFullSize(robot): + return [float(val) for val in robot.state.value]+10*[0.0] + RobotSimu.stateFullSize = stateFullSize + + robot.viewer = robotviewer.client('XML-RPC') +# robot.viewer.updateElementConfig('hrp',robot.stateFullSize()) + + def refreshView( robot ): + robot.viewer.updateElementConfig('hrp',robot.stateFullSize()) + RobotSimu.refresh = refreshView + def incrementView( robot,dt ): + robot.incrementNoView(dt) + robot.refresh() + RobotSimu.incrementNoView = RobotSimu.increment + RobotSimu.increment = incrementView + def setView( robot,*args ): + robot.setNoView(*args) + robot.refresh() + RobotSimu.setNoView = RobotSimu.set + RobotSimu.set = setView + + robot.refresh() +except: + print "No robot viewer, sorry." + robot.viewer = None + + +# --- MAIN LOOP ------------------------------------------ + +qs=[] +def inc(): + attime.run(robot.control.time+1) + robot.increment(dt) + tr.triger.recompute( robot.control.time ) + qs.append(robot.state.value) + +from ThreadInterruptibleLoop import * +@loopInThread +def loop(): + inc() + +runner=loop() + +@optionalparentheses +def go(): runner.play() +@optionalparentheses +def stop(): runner.pause() +@optionalparentheses +def next(): runner.once() + +# --- DYN ---------------------------------------------------------------------- +# --- DYN ---------------------------------------------------------------------- +# --- DYN ---------------------------------------------------------------------- + +modelDir = pkgDataRootDir[robotName] +xmlDir = pkgDataRootDir[robotName] +specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml' +jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml' + +dyn = Dynamic("dyn") +dyn.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath) +dyn.parse() + +dyn.inertiaRotor.value = inertiaRotor[robotName] +dyn.gearRatio.value = gearRatio[robotName] + +plug(robot.state,dyn.position) +dyn.velocity.value = robotDim*(0.,) +dyn.acceleration.value = robotDim*(0.,) + +dyn.ffposition.unplug() +dyn.ffvelocity.unplug() +dyn.ffacceleration.unplug() + +#dyn.setProperty('ComputeBackwardDynamics','true') +#dyn.setProperty('ComputeAccelerationCoM','true') + +robot.control.unplug() + +# --- Task Dyn ----------------------------------------- +class MetaTaskKine6d( MetaTask6d ): + def createTask(self): + self.task = Task('task'+self.name) + + def createGain(self): + self.gain = GainAdaptive('gain'+self.name) + self.gain.set(0.1,0.1,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.task.error,self.gain.error) + plug(self.gain.gain,self.task.controlGain) + def keep(self): + self.feature.position.recompute(self.dyn.position.time) + self.feature.keep() + +# Task right hand +taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist') +taskRH.ref = ((0,0,-1,0.22),(0,1,0,-0.37),(1,0,0,.74),(0,0,0,1)) +taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist') +#TODO taskLH.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. +taskRF=MetaTaskKine6d('rf',dyn,'rf','right-ankle') +taskLF=MetaTaskKine6d('lf',dyn,'lf','left-ankle') + +# --- TASK COM ------------------------------------------------------ +dyn.setProperty('ComputeCoM','true') + +featureCom = FeatureGeneric('featureCom') +featureComDes = FeatureGeneric('featureComDes') +plug(dyn.com,featureCom.errorIN) +plug(dyn.Jcom,featureCom.jacobianIN) +featureCom.sdes.value = 'featureComDes' +featureComDes.errorIN.value = (0.0478408688115,-0.0620357207995,0.684865189311) + +taskCom = Task('taskCom') +taskCom.add('featureCom') + +gCom = GainAdaptive('gCom') +plug(taskCom.error,gCom.error) +plug(gCom.gain,taskCom.controlGain) +gCom.set(1,1,1) + +# --- TASK SUPPORT -------------------------------------------------- +featureSupport = FeatureGeneric('featureSupport') +plug(dyn.com,featureSupport.errorIN) +plug(dyn.Jcom,featureSupport.jacobianIN) + +taskSupport=TaskInequality('taskSupport') +taskSupport.add(featureSupport.name) +taskSupport.selec.value = '011' +taskSupport.referenceInf.value = (-0.08,-0.045,0) # Xmin, Ymin +taskSupport.referenceSup.value = (0.11,0.335,0) # Xmax, Ymax +taskSupport.dt.value=dt + +# --- TASK POSTURE -------------------------------------------------- +featurePosture = FeatureGeneric('featurePosture') +featurePostureDes = FeatureGeneric('featurePostureDes') +plug(dyn.position,featurePosture.errorIN) +featurePosture.sdes.value = 'featurePostureDes' +featurePosture.jacobianIN.value = totuple( identity(robotDim) ) +featurePostureDes.errorIN.value = dyn.position.value + +taskPosture = Task('taskPosture') +taskPosture.add('featurePosture') + +gPosture = GainAdaptive('gPosture') +plug(taskPosture.error,gPosture.error) +plug(gPosture.gain,taskPosture.controlGain) +gPosture.set(1,1,1) + +postureSelec = range(0,3) # right leg +postureSelec += range(6,9) # left leg +postureSelec += range(12,16) # chest+head +#postureSelec += range(16,19) # right arm +#postureSelec += range(23,26) # left arm +featurePosture.selec.value = toFlags(postureSelec) + +# --- TASK JL ------------------------------------------------------ +taskJL = TaskJointLimits('taskJL') +plug(dyn.position,taskJL.position) +plug(dyn.lowerJl,taskJL.referenceInf) +plug(dyn.upperJl,taskJL.referenceSup) +taskJL.dt.value = dt +taskJL.selec.value = toFlags(range(6,robotDim)) + +# --- SOT Dyn OpSpaceH -------------------------------------- +# SOT controller. +sot = SolverKine('sot') +sot.setSize(robotDim) +#sot.damping.value = 2e-2 + +plug(sot.control,robot.control) + +def sot_addContact( sot,metaTask ): + metaTask.keep() + sot.push(metaTask.task.name) +import new +sot.addContact = new.instancemethod(sot_addContact, sot, sot.__class__) + + +# --- 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.start() + +#robot.periodicCall addSignal tr.triger + +#tr.add('p6.error','position') +tr.add('featureCom.error','comerror') +tr.add('dyn.com','com') +tr.add('dyn.position','state') +# tr.add('gCom.gain','') +# tr.add('gCom.error','gerror') + +tr.add('sot.control','') + +# --- shortcuts ------------------------------------------------- +qn=taskJL.normalizedPosition +q=taskJL.position +comref=featureComDes.errorIN + +@optionalparentheses +def iter(): print 'iter = ',robot.state.time +@optionalparentheses +def dump(): tr.dump() +@optionalparentheses +def status(): print runner.isPlay +@optionalparentheses +def iter(): print 'iter = ',robot.state.time + +# --- RUN ------------------------------------------------ + +#sot.damping.value=.1 +sot.addContact(taskLF) +sot.addContact(taskRF) +sot.push(taskCom.name) +#sot.push(taskJL.name) +#sot.push(taskSupport.name) +sot.push(taskRH.task.name) + +taskRH.ref = ((0,0,-1,0.22),(0,1,0,-0.5),(1,0,0,1.24),(0,0,0,1)) +taskRH.gain.setConstant(1) +comref.value=( 0.05, 0.1, 0.75 ) + +tr.add('taskJL.normalizedPosition','qn') + +@attime(100) +def m1(): + "Timer 1... done" + +attime(1000,stop,'pause') +attime(1000,dump,'dump') + +#matlab.prec=4 +#matlab.fullPrec=0 diff --git a/src/dynamic_graph/sot/dyninv/__init__.py b/src/dynamic_graph/sot/dyninv/__init__.py index d866a4c..d1dca2a 100755 --- a/src/dynamic_graph/sot/dyninv/__init__.py +++ b/src/dynamic_graph/sot/dyninv/__init__.py @@ -12,6 +12,8 @@ PseudoRobotDynamic('') from solver_op_space import SolverOpSpace SolverOpSpace('') +from solver_dyn_reduced import SolverDynReduced +SolverDynReduced('') from solver_kine import SolverKine SolverKine('') -- GitLab