Commit 8a6f9bd2 authored by Oscar E. Ramos P's avatar Oscar E. Ramos P
Browse files

Minor changes to the most simple kine and dyn tests

parent 96beb679
......@@ -20,13 +20,13 @@ from dynamic_graph.sot.core.utils.attime import attime
from dynamic_graph.sot.dyninv.robot_specific import pkgDataRootDir, modelName, robotDimension, initialConfig, gearRatio, inertiaRotor
from dynamic_graph.sot.dyninv.meta_task_dyn_6d import MetaTaskDyn6d
from dynamic_graph.sot.dyninv.meta_tasks_dyn import MetaTaskDynCom, MetaTaskDynPosture, AddContactHelper, gotoNd
from dynamic_graph.sot.dyninv.meta_tasks_dyn import MetaTaskDynCom, MetaTaskDynPosture, MetaTaskDynLimits, AddContactHelper, gotoNd
from numpy import *
# ------------------------------------------------------------------------------
# --- ROBOT DYNAMIC SIMULATION -------------------------------------------------
# --- ROBOT DYNAMIC SIMULATION -----------------------------------------------
# ------------------------------------------------------------------------------
robotName = 'hrp14small'
......@@ -40,14 +40,14 @@ addRobotViewer(robot,small=True,verbose=False)
dt=5e-3
# ------------------------------------------------------------------------------
# --- MAIN LOOP ----------------------------------------------------------------
# --- MAIN LOOP --------------------------------------------------------------
# ------------------------------------------------------------------------------
def inc():
robot.increment(dt)
attime.run(robot.control.time)
# verif.record()
@loopInThread
def loop():
......@@ -68,9 +68,9 @@ def iter(): print 'iter = ',robot.state.time
def status(): print runner.isPlay
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
# ----------------------------------------------------------------------------
# ---- DYN -----------------------------------------------------------------
# ----------------------------------------------------------------------------
modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
......@@ -98,10 +98,11 @@ dyn.setProperty('ComputeAccelerationCoM','true')
robot.control.unplug()
#-----------------------------------------------------------------------------
# --- OPERATIONAL TASKS (For HRP2-14)-----------------------------------------
#-----------------------------------------------------------------------------
# ----------------------------------------------------------------------------
# --- TASKS (for HRP2-14) --------------------------------------------------
# ----------------------------------------------------------------------------
# Operational Point (6D) Tasks
taskWaist = MetaTaskDyn6d('taskWaist', dyn, 'waist', 'waist')
taskChest = MetaTaskDyn6d('taskChest', dyn, 'chest', 'chest')
taskHead = MetaTaskDyn6d('taskHead', dyn, 'head', 'gaze')
......@@ -121,25 +122,12 @@ taskCom = MetaTaskDynCom(dyn,dt)
taskPosture = MetaTaskDynPosture(dyn,dt)
# Angular position and velocity limits
taskLim = TaskDynLimits('taskLim')
plug(dyn.position,taskLim.position)
plug(dyn.velocity,taskLim.velocity)
taskLim.dt.value = dt
dyn.upperJl.recompute(0)
dyn.lowerJl.recompute(0)
taskLim.referencePosInf.value = dyn.lowerJl.value
taskLim.referencePosSup.value = dyn.upperJl.value
#dqup = (0, 0, 0, 0, 0, 0, 200, 220, 250, 230, 290, 520, 200, 220, 250, 230, 290, 520, 250, 140, 390, 390, 240, 140, 240, 130, 270, 180, 330, 240, 140, 240, 130, 270, 180, 330)
dqup = (1000,)*robotDim
taskLim.referenceVelInf.value = tuple([-val*pi/180 for val in dqup])
taskLim.referenceVelSup.value = tuple([ val*pi/180 for val in dqup])
taskLim = MetaTaskDynLimits(dyn,dt)
#-----------------------------------------------------------------------------
# --- Stack of tasks controller ---------------------------------------------
#-----------------------------------------------------------------------------
# ----------------------------------------------------------------------------
# --- Dynamic Stack of Tasks (SoT) controller ------------------------------
# ----------------------------------------------------------------------------
sot = SolverDynReduced('sot')
contact = AddContactHelper(sot)
......@@ -155,9 +143,9 @@ plug(sot.solution, robot.control)
plug(sot.acceleration,robot.acceleration)
#-----------------------------------------------------------------------------
# ---- CONTACT: Contact definition -------------------------------------------
#-----------------------------------------------------------------------------
# ----------------------------------------------------------------------------
# ---- CONTACT: Contact definition -----------------------------------------
# ----------------------------------------------------------------------------
# Left foot contact
contactLF = MetaTaskDyn6d('contact_lleg',dyn,'lf','left-ankle')
......@@ -175,14 +163,14 @@ contactRF.support = ((0.11,-0.08,-0.08,0.11),(-0.045,-0.045,0.07,0.07),(-0.105,-
contactLF.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))
# Imposed erordot = 0
contactLF.feature.errordot.value=(0,0,0,0,0,0)
contactRF.feature.errordot.value=(0,0,0,0,0,0)
contactLF.feature.errordot.value = (0,0,0,0,0,0)
contactRF.feature.errordot.value = (0,0,0,0,0,0)
#-----------------------------------------------------------------------------
# --- TRACE ------------------------------------------------------------------
#-----------------------------------------------------------------------------
# -----------------------------------------------------------------------------
# --- TRACES AND AUTO RECOMPUTING SIGNALS -----------------------------------
# -----------------------------------------------------------------------------
from dynamic_graph.tracer import *
tr = Tracer('tr')
......@@ -210,12 +198,12 @@ robot.after.addSignal('dyn.lh')
robot.after.addSignal('dyn.com')
robot.after.addSignal('sot.forcesNormal')
robot.after.addSignal('dyn.waist')
robot.after.addSignal('taskLim.normalizedPosition')
robot.after.addSignal(taskLim.task.name+'.normalizedPosition')
#-----------------------------------------------------------------------------
# --- RUN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
# ----------------------------------------------------------------------------
# --- RUN ------------------------------------------------------------------
# ----------------------------------------------------------------------------
dyn.lh.recompute(0)
......@@ -228,7 +216,7 @@ sot.clear()
contact(contactLF)
contact(contactRF)
sot.push(taskLim.name)
sot.push(taskLim.task.name)
plug(robot.state,sot.position)
attime(2
......@@ -254,3 +242,4 @@ attime(400
attime(600, stop, "Stopped")
go()
# ______________________________________________________________________________
# ******************************************************************************
#
# The simplest robot task: just go and reach a point with the right hand.
#
# ______________________________________________________________________________
# ******************************************************************************
......@@ -17,29 +19,35 @@ from numpy import *
from dynamic_graph.sot.dyninv.robot_specific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
# --- ROBOT SIMU ---------------------------------------------------------------
# ------------------------------------------------------------------------------
# --- ROBOT SIMULATION ---------------------------------------------------------
# ------------------------------------------------------------------------------
robotName = 'hrp14small'
robotDim=robotDimension[robotName]
robot = RobotSimu("robot")
robotDim = robotDimension[robotName]
robot = RobotSimu("robot")
robot.resize(robotDim)
dt=5e-3
from dynamic_graph.sot.dyninv.robot_specific import halfSittingConfig
x0=-0.00949035111398315034
y0=0
z0=0.64870185118253043
x0 = -0.00949035111398315034
y0 = 0
z0 = 0.64870185118253043
halfSittingConfig[robotName] = (x0,y0,z0,0,0,0)+halfSittingConfig[robotName][6:]
q0=list(halfSittingConfig[robotName])
initialConfig[robotName]=tuple(q0)
q0 = list(halfSittingConfig[robotName])
initialConfig[robotName] = tuple(q0)
robot.set( initialConfig[robotName] )
addRobotViewer(robot,small=True,verbose=True)
addRobotViewer(robot, small=True, verbose=True)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
......@@ -48,11 +56,13 @@ def inc():
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
......@@ -67,21 +77,23 @@ plug(robot.state,dyn.position)
dyn.velocity.value = robotDim*(0.,)
dyn.acceleration.value = robotDim*(0.,)
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ------------------------------------------------------------------------------
# ---- Kinematic Stack of Tasks (SoT) -----------------------------------------
# ------------------------------------------------------------------------------
sot = SOT('sot')
sot.setNumberDofs(robotDim)
sot.setSize(robotDim)
plug(sot.control,robot.control)
# ------------------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ------------------------------------------------------------------------------
# ---- TASK GRIP ---
taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist')
handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.21)
# ---- TASK GRIP
taskRH = MetaTaskKine6d('rh',dyn,'rh','right-wrist')
handMgrip = eye(4); handMgrip[0:3,3] = (0,0,-0.21)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
......@@ -100,9 +112,12 @@ for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact.keep()
locals()['contact'+name] = contact
# --- RUN ----------------------------------------------------------------------
target=(0.5,-0.2,1.3)
# ----------------------------------------------------------------------------
# --- RUN --------------------------------------------------------------------
# ----------------------------------------------------------------------------
target = (0.5,-0.2,1.3)
robot.viewer.updateElementConfig('zmp',target+(0,0,0))
gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
......
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