Commit 298e01e7 authored by Francois Keith's avatar Francois Keith
Browse files

Update 'classic' python script for romeo flying.

parent 703a31d3
#_____________________________________________________________________________________________
#********************************************************************************************
#
# Robot motion (HRP2 14 small) using:
# Robot motion (romeo) using:
# - ONLY OPERATIONAL TASKS
# - Joint limits (position and velocity)
#_____________________________________________________________________________________________
......@@ -17,17 +17,16 @@ from dynamic_graph.sot.dyninv import *
import dynamic_graph.script_shortcuts
from dynamic_graph.script_shortcuts import optionalparentheses
from dynamic_graph.matlab import matlab
sys.path.append('..')
from dynamic_graph.sot.core.meta_task_6d import toFlags
from meta_task_dyn_6d import MetaTaskDyn6d
from meta_tasks_dyn import *
from attime import attime
from dynamic_graph.sot.dyninv.meta_task_dyn_6d import MetaTaskDyn6d
from dynamic_graph.sot.dyninv.meta_tasks_dyn import *
from dynamic_graph.sot.core.utils.attime import attime
from numpy import *
from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor, specificitiesName, jointRankName, postureRange
from matrix_util import matrixToTuple, vectorToTuple,rotate
from history import History
from zmp_estimator import ZmpEstimator
from viewer_helper import addRobotViewer,VisualPinger
from dynamic_graph.sot.dyninv.robot_specific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor, specificitiesName, jointRankName, postureRange
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
# from dynamic_graph.sot.core.utils.history import History
# from dynamic_graph.sot.dyninv.zmp_estimator import ZmpEstimator
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger
#-----------------------------------------------------------------------------
# --- ROBOT SIMU -------------------------------------------------------------
......@@ -39,10 +38,7 @@ robotDim = robotDimension[robotName]
RobotClass = RobotDynSimu
robot = RobotClass("robot")
robot.resize(robotDim)
addRobotViewer(robot,small=False,verbose=False)
dt = 5e-3
initialConfig['hrp14small'] = (-0.033328803958899381, -0.0019839923040723341, 0.62176527349722499, 2.379901541270165e-05, 0.037719492175904465, 0.00043085147714449579, -0.00028574496353126724, 0.0038294370786961648, -0.64798319906979551, 1.0552418879542016, -0.44497846451873851, -0.0038397195926379991, -0.00028578259876671871, 0.0038284398205732629, -0.64712828871069394, 1.0534202525984278, -0.4440117393779604, -0.0038387216246160054, 0.00014352031102944824, 0.013151503268540811, -0.00057411504064861592, -0.050871000025766742, 0.21782780288481224, -0.37965640592672439, -0.14072647716213352, -1.1942332339530364, 0.0055454863752273523, -0.66956710808008013, 0.1747981826611808, 0.21400703176352612, 0.38370527720078107, 0.14620204468509851, -1.1873407322935838, -0.0038746980026940735, -0.66430172366423146, 0.17500428384087438)
addRobotViewer(robot,small=True,small_extra=24,verbose=False)
# Similar initial position with hand forward
robot.set(initialConfig[robotName])
......@@ -51,17 +47,18 @@ robot.set(initialConfig[robotName])
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
dt = 5e-3
def inc():
robot.increment(dt)
# Execute a function at time t, if specified with t.add(...)
if 'refresh' in ZmpEstimator.__dict__: zmp.refresh()
# if 'refresh' in ZmpEstimator.__dict__: zmp.refresh()
attime.run(robot.control.time)
robot.viewer.updateElementConfig('zmp',[zmp.zmp.value[0],zmp.zmp.value[1],0,0,0,0])
# robot.viewer.updateElementConfig('zmp',[zmp.zmp.value[0],zmp.zmp.value[1],0,0,0,0])
if dyn.com.time >0:
robot.viewer.updateElementConfig('com',[dyn.com.value[0],dyn.com.value[1],0,0,0,0])
history.record()
# history.record()
from ThreadInterruptibleLoop import *
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def loop():
# try:
......@@ -77,26 +74,6 @@ def stop(): runner.pause()
@optionalparentheses
def next(): inc()
# --- shortcuts -------------------------------------------------
@optionalparentheses
def q():
print robot.state.__repr__()
@optionalparentheses
def qdot(): print robot.control.__repr__()
@optionalparentheses
def iter(): print 'iter = ',robot.state.time
@optionalparentheses
def status(): print runner.isPlay
@optionalparentheses
def pl(): print matlab( matrixToTuple(zmp.leftSupport()) ).resstr
@optionalparentheses
def pr(): print matlab( matrixToTuple(zmp.righttSupport()) ).resstr
@optionalparentheses
def dump():
history.dumpToOpenHRP('openhrp/planche')
attime.addPing( VisualPinger(robot.viewer) )
#-----------------------------------------------------------------------------
......@@ -216,8 +193,8 @@ contactLF.support = ((0.11,-0.08,-0.08,0.11),(-0.07,-0.07,0.045,0.045),(-0.105,-
contactLF.support = ((0.03,-0.03,-0.03,0.03),(-0.015,-0.015,0.015,0.015),(-0.105,-0.105,-0.105,-0.105))
#--- ZMP ---------------------------------------------------------------------
zmp = ZmpEstimator('zmp')
zmp.declare(sot,dyn)
# zmp = ZmpEstimator('zmp')
# zmp.declare(sot,dyn)
#-----------------------------------------------------------------------------
# --- TRACE ------------------------------------------------------------------
......@@ -231,7 +208,7 @@ tr.add('dyn.com','com')
tr.add(taskCom.feature.name+'.error','ecom')
tr.add('dyn.waist','waist')
tr.add('dyn.rh','rh')
tr.add('zmp.zmp','')
# tr.add('zmp.zmp','')
tr.add('dyn.position','')
tr.add('dyn.velocity','')
tr.add('robot.acceleration','robot_acceleration')
......@@ -254,7 +231,7 @@ robot.after.addSignal('dyn.waist')
robot.after.addSignal('taskLim.normalizedPosition')
tr.add('taskLim.normalizedPosition','qn')
history = History(dyn,1,zmp.zmp)
# history = History(dyn,1,zmp.zmp)
#-----------------------------------------------------------------------------
# --- RUN --------------------------------------------------------------------
......@@ -342,7 +319,7 @@ if WITH_FULL_EXTENTION:
,lambda: taskPosture.gotoq(chest=(0,),rleg=(0,0,0,MAX_EXT,0,0),head=(0,0,0,0),rarm=(0,-pi/2,0,0,0,0,0),larm=(0,pi/2,0,0,0,0,0)), "extend arm")
attime(2080
,lambda: taskPosture.gotoq(30,chest=(0,),rleg=(0,0,0,MAX_EXT,0.73,0),head=(0,-pi/4,0,0),rarm=(0,-pi/2,0,0,0,0,0),larm=(0,pi/2,0,0,0,0,0)), "extend foot")
,lambda: taskPosture.gotoq(chest=(0,),rleg=(0,0,0,MAX_EXT,0.73,0),head=(0,-pi/4,0,0),rarm=(0,-pi/2,0,0,0,0,0),larm=(0,pi/2,0,0,0,0,0)), "extend foot")
tex=1000
else: tex=0
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment