From af339ea89464bc86cc651733a839c6f1ade18d0a Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Fri, 18 Jan 2013 11:44:21 +0100
Subject: [PATCH] Corrected the unittest kinewalk.
---
python/{unitests => jumble}/dyndebug.py | 0
python/{unitests => jumble}/dynpg.py | 0
python/{unitests => jumble}/dynreduced.py | 0
python/{unitests => jumble}/dynwalk.py | 0
python/{unitests => jumble}/kinedebug.py | 0
python/{unitests => jumble}/kinemorisawa.py | 0
python/{unitests => jumble}/kinesimple.py | 0
python/{unitests => jumble}/romeo/planche.py | 0
python/{unitests => jumble}/step.py | 0
python/{unitests => jumble}/walktrans.py | 0
python/unitests/kinewalk.py | 162 -------------------
python/{jumble => unittests}/kinewalk.py | 0
12 files changed, 162 deletions(-)
rename python/{unitests => jumble}/dyndebug.py (100%)
rename python/{unitests => jumble}/dynpg.py (100%)
rename python/{unitests => jumble}/dynreduced.py (100%)
rename python/{unitests => jumble}/dynwalk.py (100%)
rename python/{unitests => jumble}/kinedebug.py (100%)
rename python/{unitests => jumble}/kinemorisawa.py (100%)
rename python/{unitests => jumble}/kinesimple.py (100%)
rename python/{unitests => jumble}/romeo/planche.py (100%)
rename python/{unitests => jumble}/step.py (100%)
rename python/{unitests => jumble}/walktrans.py (100%)
delete mode 100644 python/unitests/kinewalk.py
rename python/{jumble => unittests}/kinewalk.py (100%)
diff --git a/python/unitests/dyndebug.py b/python/jumble/dyndebug.py
similarity index 100%
rename from python/unitests/dyndebug.py
rename to python/jumble/dyndebug.py
diff --git a/python/unitests/dynpg.py b/python/jumble/dynpg.py
similarity index 100%
rename from python/unitests/dynpg.py
rename to python/jumble/dynpg.py
diff --git a/python/unitests/dynreduced.py b/python/jumble/dynreduced.py
similarity index 100%
rename from python/unitests/dynreduced.py
rename to python/jumble/dynreduced.py
diff --git a/python/unitests/dynwalk.py b/python/jumble/dynwalk.py
similarity index 100%
rename from python/unitests/dynwalk.py
rename to python/jumble/dynwalk.py
diff --git a/python/unitests/kinedebug.py b/python/jumble/kinedebug.py
similarity index 100%
rename from python/unitests/kinedebug.py
rename to python/jumble/kinedebug.py
diff --git a/python/unitests/kinemorisawa.py b/python/jumble/kinemorisawa.py
similarity index 100%
rename from python/unitests/kinemorisawa.py
rename to python/jumble/kinemorisawa.py
diff --git a/python/unitests/kinesimple.py b/python/jumble/kinesimple.py
similarity index 100%
rename from python/unitests/kinesimple.py
rename to python/jumble/kinesimple.py
diff --git a/python/unitests/romeo/planche.py b/python/jumble/romeo/planche.py
similarity index 100%
rename from python/unitests/romeo/planche.py
rename to python/jumble/romeo/planche.py
diff --git a/python/unitests/step.py b/python/jumble/step.py
similarity index 100%
rename from python/unitests/step.py
rename to python/jumble/step.py
diff --git a/python/unitests/walktrans.py b/python/jumble/walktrans.py
similarity index 100%
rename from python/unitests/walktrans.py
rename to python/jumble/walktrans.py
diff --git a/python/unitests/kinewalk.py b/python/unitests/kinewalk.py
deleted file mode 100644
index dabf31a..0000000
--- a/python/unitests/kinewalk.py
+++ /dev/null
@@ -1,162 +0,0 @@
-# ______________________________________________________________________________
-# ******************************************************************************
-# ______________________________________________________________________________
-# ******************************************************************************
-
-sys.path.append('..')
-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 matrix_util import matrixToTuple
-from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags
-from dynamic_graph.sot.core.meta_tasks_kine import *
-from viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
-from attime import attime
-from numpy import *
-
-from history import History
-
-from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
-
-# --- ROBOT SIMU ---------------------------------------------------------------
-# --- ROBOT SIMU ---------------------------------------------------------------
-# --- ROBOT SIMU ---------------------------------------------------------------
-
-robotName = 'hrp14small'
-robotDim=robotDimension[robotName]
-robot = RobotSimu("robot")
-robot.resize(robotDim)
-dt=5e-3
-
-from robotSpecific import halfSittingConfig
-x0=-0.00949035111398315034
-y0=0
-z0=0.64870185118253043 #0.6487018512
-halfSittingConfig[robotName] = (x0,y0,z0,0,0,0)+halfSittingConfig[robotName][6:]
-
-initialConfig[robotName]=halfSittingConfig[robotName]
-
-robot.set( initialConfig[robotName] )
-addRobotViewer(robot,small=True,verbose=False)
-
-#-------------------------------------------------------------------------------
-#----- MAIN LOOP ---------------------------------------------------------------
-#-------------------------------------------------------------------------------
-from ThreadInterruptibleLoop import loopInThread,loopShortcuts
-@loopInThread
-def inc():
- robot.increment(dt)
- attime.run(robot.control.time)
- updateComDisplay(robot,dyn.com)
-
-runner=inc()
-[go,stop,next,n]=loopShortcuts(runner)
-
-#-----------------------------------------------------------------------------
-#---- 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()
-
-# --- PG ---------------------------------------------------------
-# --- PG ---------------------------------------------------------
-# --- PG ---------------------------------------------------------
-from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG
-pg = MetaPG(dyn)
-pg.plugZmp(robot)
-
-
-# ---- SOT ---------------------------------------------------------------------
-# ---- SOT ---------------------------------------------------------------------
-# ---- SOT ---------------------------------------------------------------------
-# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
-from dynamic_graph.sot.dyninv import SolverKine
-sot = SolverKine('sot')
-sot.setSize(robotDim)
-plug(sot.control,robot.control)
-
-# ---- TASKS -------------------------------------------------------------------
-# ---- TASKS -------------------------------------------------------------------
-# ---- TASKS -------------------------------------------------------------------
-
-# ---- WAIST TASK ---
-taskWaist=MetaTask6d('waist',dyn,'waist','waist')
-pg.plugWaistTask(taskWaist)
-taskWaist.task.controlGain.value = 5
-
-# --- TASK COM ---
-taskCom = MetaTaskKineCom(dyn,"compd")
-plug(pg.comRef.ref,taskCom.featureDes.errorIN)
-plug(pg.pg.dcomref,taskCom.featureDes.errordotIN)
-taskCom.task = TaskPD('taskComPD')
-taskCom.task.add(taskCom.feature.name)
-plug(taskCom.feature.errordot,taskCom.task.errorDot)
-plug(taskCom.task.error,taskCom.gain.error)
-plug(taskCom.gain.gain,taskCom.task.controlGain)
-taskCom.gain.setConstant(40)
-taskCom.task.setBeta(-1)
-
-# --- TASK FEET
-taskRF=MetaTask6d('rf',dyn,'rf','right-ankle')
-plug(pg.pg.rightfootref,taskRF.featureDes.position)
-taskRF.task.controlGain.value = 5
-
-taskLF=MetaTask6d('lf',dyn,'lf','left-ankle')
-plug(pg.pg.leftfootref,taskLF.featureDes.position)
-taskLF.task.controlGain.value = 5
-
-# --- TRACER -----------------------------------------------------------------
-from dynamic_graph.tracer import *
-tr = Tracer('tr')
-tr.open('/tmp/','','.dat')
-tr.start()
-robot.after.addSignal('tr.triger')
-
-tr.add(taskRF.featureDes.name+'.position','refr')
-tr.add(taskLF.featureDes.name+'.position','refl')
-
-# --- RUN ----------------------------------------------------------------------
-# --- RUN ----------------------------------------------------------------------
-# --- RUN ----------------------------------------------------------------------
-
-sot.push(taskWaist.task.name)
-sot.push(taskRF.task.name)
-sot.push(taskLF.task.name)
-sot.push(taskCom.task.name)
-
-# --- HERDT PG AND START -------------------------------------------------------
-# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
-pg.startHerdt()
-# You can now modifiy the speed of the robot using set pg.pg.velocitydes [3]( x, y, yaw)
-pg.pg.velocitydes.value =(0.1,0.0,0.0)
-
-
-go()
-
diff --git a/python/jumble/kinewalk.py b/python/unittests/kinewalk.py
similarity index 100%
rename from python/jumble/kinewalk.py
rename to python/unittests/kinewalk.py
--
GitLab