Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • gsaurel/sot-dyninv
1 result
Show changes
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 6798 additions and 0 deletions
python/2013_coursens/p105_demo_romeo.py 0 → 100644
View file @ 60cf95b2
# ______________________________________________________________________________
# ******************************************************************************
# ______________________________________________________________________________
# ******************************************************************************
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.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.meta_tasks_kine import *
from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
from dynamic_graph.sot.core.meta_task_visual_point import MetaTaskVisualPoint
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
from dynamic_graph.sot.core.utils.attime import attime,ALWAYS,refset,sigset
from numpy import *
from dynamic_graph.sot.core.utils.history import History
from dynamic_graph.sot.dyninv.robot_specific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor,specificitiesName,jointRankName
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
robotName = 'romeo'
robotDim=robotDimension[robotName]
robot = RobotSimu('romeo')
robot.resize(robotDim)
dt=5e-3
from dynamic_graph.sot.dyninv.robot_specific import halfSittingConfig
x0=-0.00949035111398315034
y0=0
z0=0.64870185118253043
# halfSittingConfig[robotName] = (x0,y0,z0,0,0,0)+initialConfig[robotName][6:]
halfSittingConfig[robotName] = initialConfig[robotName]
q0=list(halfSittingConfig[robotName])
initialConfig[robotName]=tuple(q0)
robot.set( initialConfig[robotName] )
addRobotViewer(robot,small=True,small_extra=24,verbose=False)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
robot.increment(dt)
attime.run(robot.control.time)
updateComDisplay(robot,dyn.com)
history.record()
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/' + specificitiesName[robotName]
jointRankPath = xmlDir + '/' + jointRankName[robotName]
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()
# ---- 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
def toList(sot):
return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
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.07,-0.02,0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist')
taskLH.opmodif = matrixToTuple(handMgrip)
taskLH.feature.frame('desired')
# --- STATIC COM (if not walking)
taskCom = MetaTaskKineCom(dyn)
# --- TASK AVOID
taskShoulder=MetaTaskKine6d('shoulder',dyn,'shoulder','LShoulderPitch')
taskShoulder.feature.frame('desired')
gotoNd(taskShoulder,(0,0,0),'010')
taskShoulder.task = TaskInequality('taskShoulderAvoid')
taskShoulder.task.add(taskShoulder.feature.name)
taskShoulder.task.referenceInf.value = (-10,) # Xmin, Ymin
taskShoulder.task.referenceSup.value = (0.20,) # Xmin, Ymin
taskShoulder.task.dt.value=dt
taskShoulder.task.controlGain.value = 0.9
taskElbow=MetaTaskKine6d('elbow',dyn,'elbow','LElbowYaw')
taskElbow.feature.frame('desired')
gotoNd(taskElbow,(0,0,0),'010')
taskElbow.task = TaskInequality('taskElbowAvoid')
taskElbow.task.add(taskElbow.feature.name)
taskElbow.task.referenceInf.value = (-10,) # Xmin, Ymin
taskElbow.task.referenceSup.value = (0.20,) # Xmin, Ymin
taskElbow.task.dt.value=dt
taskElbow.task.controlGain.value = 0.9
# --- 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.15,0) # Xmin, Ymin
taskSupport.referenceSup.value = (0.11,0.15,0) # Xmax, Ymax
taskSupport.dt.value=dt
# --- TASK SUPPORT SMALL --------------------------------------------
featureSupportSmall = FeatureGeneric('featureSupportSmall')
plug(dyn.com,featureSupportSmall.errorIN)
plug(dyn.Jcom,featureSupportSmall.jacobianIN)
taskSupportSmall=TaskInequality('taskSupportSmall')
taskSupportSmall.add(featureSupportSmall.name)
taskSupportSmall.selec.value = '011'
#taskSupportSmall.referenceInf.value = (-0.02,-0.02,0) # Xmin, Ymin
#askSupportSmall.referenceSup.value = (0.02,0.02,0) # Xmax, Ymax
taskSupportSmall.referenceInf.value = (-0.02,-0.05,0) # Xmin, Ymin
taskSupportSmall.referenceSup.value = (0.02,0.05,0) # Xmax, Ymax
taskSupportSmall.dt.value=dt
# --- POSTURE ---
taskPosture = MetaTaskKinePosture(dyn)
# --- GAZE ---
taskGaze = MetaTaskVisualPoint('gaze',dyn,'head','gaze')
# Head to camera matrix transform
# Camera RU headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]])
# Camera LL
headMcam=array([[0.0,0.0,1.0,0.081],[1.,0.0,0.0,0.072],[0.0,1.,0.0,0.031],[0.0,0.0,0.0,1.0]])
headMcam = dot(headMcam,rotate('x',10*pi/180))
taskGaze.opmodif = matrixToTuple(headMcam)
# --- FOV ---
taskFoV = MetaTaskVisualPoint('FoV',dyn,'head','gaze')
taskFoV.opmodif = matrixToTuple(headMcam)
taskFoV.task=TaskInequality('taskFoVineq')
taskFoV.task.add(taskFoV.feature.name)
[Xmax,Ymax]=[0.38,0.28]
taskFoV.task.referenceInf.value = (-Xmax,-Ymax) # Xmin, Ymin
taskFoV.task.referenceSup.value = (Xmax,Ymax) # Xmax, Ymax
taskFoV.task.dt.value=dt
taskFoV.task.controlGain.value=0.01
taskFoV.featureDes.xy.value = (0,0)
# --- Task Joint Limits -----------------------------------------
dyn.upperJl.recompute(0)
dyn.lowerJl.recompute(0)
taskJL = TaskJointLimits('taskJL')
plug(dyn.position,taskJL.position)
taskJL.controlGain.value = 10
taskJL.referenceInf.value = dyn.lowerJl.value
taskJL.referenceSup.value = dyn.upperJl.value
taskJL.dt.value = dt
taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,dyn,name,joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
locals()['contact'+name] = contact
# --- TRACER -----------------------------------------------------------------
from dynamic_graph.tracer import *
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
tr.start()
robot.after.addSignal('tr.triger')
tr.add('dyn.com','com')
history = History(dyn,1)
# --- SHORTCUTS ----------------------------------------------------------------
qn = taskJL.normalizedPosition
@optionalparentheses
def pqn(details=True):
''' Display the normalized configuration vector. '''
qn.recompute(robot.state.time)
s = [ "{0:.1f}".format(v) for v in qn.value]
if details:
print("Rleg: "+" ".join(s[:6]))
print("Lleg: "+" ".join(s[6:12]))
print("Body: "+" ".join(s[12:16]))
print("Rarm: "+" ".join(s[16:23]))
print("Larm :"+" ".join(s[23:30]))
else:
print(" ".join(s[:30]))
def jlbound(t=None):
'''Display the velocity bound induced by the JL as a double-column matrix.'''
if t==None: t=robot.state.time
taskJL.task.recompute(t)
return matrix([ [float(x),float(y)] for x,y
in [ c.split(',') for c in taskJL.task.value[6:-3].split('),(') ] ])
def p6d(R,t):
M=eye(4)
M[0:3,0:3]=R
M[0:3,3]=t
return M
def push(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName not in sot.toList():
sot.push(taskName)
if taskName!="taskposture" and "taskposture" in sot.toList():
sot.down("taskposture")
def pop(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName in sot.toList(): sot.rm(taskName)
# --- DISPLAY ------------------------------------------------------------------
# --- DISPLAY ------------------------------------------------------------------
# --- DISPLAY ------------------------------------------------------------------
RAD=pi/180
comproj = [0.1,-0.95,1.6]
#robot.viewer.updateElementConfig('footproj',[0.5,0.15,1.6+0.08,0,-pi/2,0 ])
robot.viewer.updateElementConfig('footproj',comproj+[0,-pi/2,0 ])
robot.viewer.updateElementConfig('zmp2',[0,0,-10,0,0,0])
class BallPosition:
def __init__(self,xyz=(0,-1.1,0.9)):
self.ball = xyz
self.prec = 0
self.t = 0
self.duration = 0
self.f = 0
self.xyz= self.ball
def move(self,xyz,duration=50):
self.prec = self.ball
self.ball = xyz
self.t = 0
self.duration = duration
self.changeTargets()
if duration>0:
self.f = lambda : self.moveDisplay()
attime(ALWAYS,self.f)
else:
self.moveDisplay()
def moveDisplay(self):
tau = 1.0 if self.duration<=0 else float(self.t) / self.duration
xyz = tau * array(self.ball) + (1-tau) * array(self.prec)
robot.viewer.updateElementConfig('zmp',vectorToTuple(xyz)+(0,0,0))
self.t += 1
if self.t>self.duration and self.duration>0:
attime.stop(self.f)
self.xyz= xyz
def changeTargets(self):
gotoNd(taskRH,self.ball,'111',(4.9,0.9,0.01,0.9))
taskFoV.goto3D(self.ball)
b = BallPosition()
tr.add(taskJL.name+".normalizedPosition","qn")
tr.add('dyn.com','com')
tr.add(taskRH.task.name+'.error','erh')
tr.add(taskFoV.feature.name+'.error','fov')
tr.add(taskFoV.task.name+'.normalizedPosition','fovn')
tr.add(taskSupportSmall.name+".normalizedPosition",'comsmalln')
tr.add(taskSupport.name+".normalizedPosition",'comn')
robot.after.addSignal(taskJL.name+".normalizedPosition")
robot.after.addSignal(taskSupportSmall.name+".normalizedPosition")
robot.after.addSignal(taskFoV.task.name+".normalizedPosition")
robot.after.addSignal(taskFoV.feature.name+'.error')
robot.after.addSignal(taskSupport.name+".normalizedPosition")
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
dyn.com.recompute(0)
taskCom.featureDes.errorIN.value = dyn.com.value
taskCom.task.controlGain.value = 10
ball = BallPosition((0,-1.1,0.9))
push(taskRH)
ball.move((0.5,-0.2,1.0),0)
next()
next()
next()
def config(ref=0):
if ref==0:
print '''Only the task RH'''
elif ref==1:
print '''Task RH + foot constraint, balance is kept'''
sot.addContact(contactRF)
sot.addContact(contactLF)
elif ref==2:
print '''Task RH + foot constraint, balance is lost'''
sot.addContact(contactRF)
sot.addContact(contactLF)
ball.move((-0.15,-0.2,1.3),0)
print 'pouet'
elif ref==3:
print '''Task RH + foot constraint + COM='''
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskCom)
ball.move((0.15,0.1,1),0)
elif ref==4:
print '''Task RH + foot constraint + COM= + JL'''
qu = list(dyn.upperJl.value)
qu[19]=0
taskJL.referenceSup.value =tuple(qu)
push(taskJL)
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskCom)
ball.move((0.15,0.1,1),0)
elif ref==5:
print '''Task RH + foot constraint + COM<'''
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskSupport)
ball.move((0.15,-0.2,1.3),0)
elif ref==6:
print '''Task RH + foot constraint + COM= + SINGULARITE '''
print '''(press 4x i to reach it)'''
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskCom)
ball.move((0.15,-0.2,1.3),0)
elif ref==7:
print '''Task RH + foot constraint + COM= + SINGULARITE + DAMPING'''
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskCom)
ball.move((0.15,-0.2,1.3),0)
sot.down(taskRH.task.name)
sot.down(taskRH.task.name)
sot.damping.value = 0.1
else:
print '''Not a correct config.'''
return
go()
c=config
@optionalparentheses
def i():
xyz=ball.xyz
xyz[0] += 0.1
ball.move(vectorToTuple(xyz),30)
def menu(ref=0):
print '\n\n\n\n\n\n\n\n\n'
print '''0: Only the task RH'''
print '''1: Task RH + foot constraint, balance is kept'''
print '''2: Task RH + foot constraint, balance is lost'''
print '''3: Task RH + foot constraint + COM='''
print '''4: Task RH + foot constraint + COM= + JL'''
print '''5: Task RH + foot constraint + COM<'''
print '''6: Task RH + foot constraint + COM= + SINGULARITE '''
print '''7: Task RH + foot constraint + COM= + SINGULARITE + DAMPING'''
print ''
uinp = raw_input(' Config? >> ')
config(int(uinp))
menu()
python/2013_coursens/p105_walk.py 0 → 100644
View file @ 60cf95b2
# ______________________________________________________________________________
# ******************************************************************************
# ______________________________________________________________________________
# ******************************************************************************
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.matrix_util import matrixToTuple, vectorToTuple
from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.meta_tasks_kine import *
from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
from dynamic_graph.sot.core.utils.attime import attime,ALWAYS,refset,sigset
from numpy import *
from dynamic_graph.sot.core.utils.history import History
from dynamic_graph.sot.dyninv.robot_specific 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 dynamic_graph.sot.dyninv.robot_specific 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]
initialConfig[robotName]=(-1.9487374883906774e-27, -1.7176468466207308e-27, 0.63516120560912981, 4.9861897636537844e-20, -1.0588941103603052e-18, 1.2364668742710116e-19, -4.8902218174850969e-18, 2.4342667073361099e-18, -0.48729641533290902, 0.97459283066581781, -0.48729641533290879, -9.861508377534219e-19, -2.359096854752087e-18, -1.3129513558333748e-17, -0.48729641533290935, 0.97459283066581859, -0.48729641533290929, 1.6873551935899719e-17, 0.050059280167850009, 0.010000806681109737, -0.12770543788413571, 0.21393414582231227, 0.1568991096333438, -0.12050409251208098, -0.036557035976426684, -1.0972074690967477, -0.01444727776027852, -0.64702871259334449, 0.17550310601489358, 0.90762433294316336, 0.14017481565796389, 0.0052476440775108182, -0.23339033298135339, -0.00061821588820589444, 0.054877110564186989, 0.17414673138839104)
# Work nicely, but q0 is uggly.
initialConfig[robotName]=(0.00049921279479388854, 0.00049996060172474786, 0.63500107965860886, 8.1671945780155143e-23, 1.5720102693719503e-23, -4.8579693842636446e-23, 9.5431180449191677e-21, -0.00094329030861069489, -0.48680514099116468, 0.97549398127625753, -0.48868884028509291, 0.00094329030861069489, 6.989934206013467e-21, -0.00094334907227440175, -0.48703981005325936, 0.97596355410326363, -0.48892374405000433, 0.00094334907227440175, 0.11348807526440016, 0.20083116658525665, -0.0010545697535446316, -0.029186560447927903, 0.32171157730517314, -0.14204521593028116, -0.10741283049826153, -0.68485604962994939, -0.044874689963830129, -0.078235128020448866, 0.17272683925193663, 0.94207249294658169, 0.23722916347594097, 0.02069536375553379, -0.42067142923482798, -0.00032889180672932412, 0.019378691610578484, 0.17645563918444634)
initialConfig[robotName]=(0.00049999999992054859, 0.00049999999999627166, 0.6350000000004179, -1.3576245230323505e-16, -1.2271661091679395e-14, -7.0755021448112835e-21, 1.1590588873416236e-17, -0.00094336656172265174, -0.48680748131139595, 0.97550163614052687, -0.48869415482911871, 0.00094336656172278759, 1.159528247642137e-17, -0.0009434253350073629, -0.48704216746084605, 0.97597124353230558, -0.48892907607144726, 0.00094342533500749864, 0.0, 0.0, 0.0, 0.0, 0.26179938779914941, -0.17453292519943295, 0.0, -0.52359877559829882, 0.0, 0.0, 0.17453292519943295, 0.26179938779914941, 0.17453292519943295, 0.0, -0.52359877559829882, 0.0, 0.0, 0.17453292519943295)
robot.set( initialConfig[robotName] )
addRobotViewer(robot,small=True,verbose=True)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
from dynamic_graph.sot.core.utils.thread_interruptible_loop 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 -------------------------------------------------------------------
# ---- TASK GRIP ---
taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist')
handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.17)
taskRH.opmodif = matrixToTuple(handMgrip)
# --- STATIC COM (if not walking)
taskCom0 = MetaTaskKineCom(dyn)
# --- POSTURE ---
taskPosture = MetaTaskKinePosture(dyn)
# --- GAZE ---
taskGaze = MetaTaskKine6d('gaze',dyn,'head','gaze')
# Head to camera matrix transform
headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]])
taskGaze.opmodif = matrixToTuple(headMcam)
taskGaze.feature.frame('current')
taskGaze.feature.selec.value = '011'
# --- Task Joint Limits -----------------------------------------
dyn.upperJl.recompute(0)
dyn.lowerJl.recompute(0)
taskJL = TaskJointLimits('taskJL')
plug(dyn.position,taskJL.position)
taskJL.controlGain.value = 10
taskJL.referenceInf.value = dyn.lowerJl.value
#taskJL.referenceInf.value = ( 0, 0, 0, 0, 0, 0, -0.785398163000, -0.610865238000, -2.181661565000, -0.034906585000, -1.308996939000, -0.349065850000, -0.523598775000, -0.349065850000, -2.181661565000, -0.034906585000, -1.308996939000, -0.610865238000, -0.785398163000, 0.01, -0.785398163000, -0.523598775000, -3.141592654000, -1.658062789000, -1.605702912000, -2.391101075000, -1.605702912000, -1.518400000000, 0, -3.141592654000, -0.174532925000, -1.605702912000, -2.391101075000, -1.605702912000, -1.605702912000, 0 )
taskJL.referenceSup.value = dyn.upperJl.value
taskJL.dt.value = dt
taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
# ---- 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)
taskCom.feature.selec.value = '011'
# --- 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
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,dyn,name,joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
locals()['contact'+name] = contact
# --- 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')
tr.add('dyn.com','com')
tr.add(taskRH.task.name+'.error','erh')
tr.add(taskJL.name+".normalizedPosition","qn")
tr.add("robot.state","q")
tr.add("robot.control","qdot")
robot.after.addSignal(taskJL.name+".normalizedPosition")
# --- SHORTCUTS ----------------------------------------------------------------
qn = taskJL.normalizedPosition
@optionalparentheses
def pqn(details=True):
''' Display the normalized configuration vector. '''
qn.recompute(robot.state.time)
s = [ "{0:.1f}".format(v) for v in qn.value]
if details:
print("Rleg: "+" ".join(s[:6]))
print("Lleg: "+" ".join(s[6:12]))
print("Body: "+" ".join(s[12:16]))
print("Rarm: "+" ".join(s[16:22]))
print("Larm :"+" ".join(s[22:28]))
else:
print(" ".join(s[:30]))
def jlbound(t=None):
'''Display the velocity bound induced by the JL as a double-column matrix.'''
if t==None: t=robot.state.time
taskJL.task.recompute(t)
return matrix([ [float(x),float(y)] for x,y
in [ c.split(',') for c in taskJL.task.value[6:-3].split('),(') ] ])
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
#sot.addContact(contactRF)
#sot.addContact(contactLF)
#gotoNd(taskWaist,(0,0,0),'111011',(1,))
#taskWaist.feature.frame('desired')
#sot.push(taskWaist.task.name)
#taskCom0.featureDes.errorIN.value = dyn.com.value
#taskCom0.task.controlGain.value = 10
#sot.push(taskCom0.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)
#ball0 = (2.5,-0.3,0.5)
ball0 = (0.5,-2.3,0.5)
w=(1/1.3,1/1.8,1/3.9)
h=(0.1,0.07,0.3)
sot.damping.value = 5e-2
sot.push(taskJL.name)
sot.push(taskWaist.task.name)
sot.push(taskRF.task.name)
sot.push(taskLF.task.name)
sot.push(taskCom.task.name)
taskPosture.gotoq((5,1,0.05,0.9),rhand=[1,])
sot.push(taskPosture.task.name)
gotoNd(taskGaze,ball0,'011',(10,1,0.01,0.9))
sot.push(taskGaze.task.name)
gotoNd(taskRH,ball0,'011',(10,1,0.01,0.9))
sot.push(taskRH.task.name)
# --- SCRIPT EVENTS ----------------------------------------------------------
#from random import random
import numpy.random as nprand
class BallTraj:
def __init__(self,dt,ball0,v0=(0.0,0.0,0.0)):
self.dt = dt
self.pball=array(ball0)
self.vball=array(v0)
self.bounce = 0
self.z0 = 0.02
self.elasticZ = 0.99
self.elasticXY = 0.3
self.gravity=(0,0,-3.0)
nprand.seed(4)
def __call__(self,t):
self.vball += self.dt*array(self.gravity)
self.pball += self.dt*self.vball
if self.pball[2]<self.z0:
self.vball[2] = self.elasticZ * abs(self.vball[2])
self.pball[2] = self.z0
self.vball[0:2] = (nprand.rand(2)-0.5)*self.elasticXY
return vectorToTuple(self.pball)
ballTraj = BallTraj(dt,ball0)
def trackTheBall(ball=None):
if ball==None:
t=robot.state.time/200.0
#ball = h * cos(array(w)*t) + ball0
ball = ballTraj(t)
if isinstance(ball,ndarray): ball=vectorToTuple(ball)
gotoNd(taskRH,ball)
gotoNd(taskGaze,ball)
robot.viewer.updateElementConfig('zmp',ball+(0,0,0))
fixed=False
if fixed: trackTheBall()
else: attime(ALWAYS,trackTheBall)
def ballInHand():
#rh = array(dyn.rh.value)[0:3,3]
robot.after.addSignal(taskRH.feature.name + '.position')
rh = array(taskRH.feature.position.value)[0:3,3]
robot.viewer.updateElementConfig('zmp',vectorToTuple(rh)+(0,0,0))
#@attime(3450)
def goForTheBall():
'''The hand goes for the hand.'''
taskRH.feature.selec.value = '111'
setGain(taskRH.gain,(5,0.5,0.03,0.9))
taskWaist.feature.selec.value = '011000'
ballTraj.elasticXY = 0.0
ballTraj.gravity = (0,0,-3.0)
attime(ALWAYS,graspIf)
#@attime(3400)
def clearAttimePeriodic():
'''Stop the PG.'''
del attime.events[ALWAYS]
attime(ALWAYS,trackTheBall)
pg.pg.velocitydes.value = (0,0,0)
#@attime(3900)
def grasp():
'''Ball is in the hand.'''
del attime.events[ALWAYS]
attime(ALWAYS,ballInHand)
sot.rm(taskRH.task.name)
sot.rm(taskGaze.task.name)
q0 = halfSittingConfig[robotName]
taskPosture.gotoq((5,1,0.05,0.9),q0,rarm=[],larm=[],chest=[],head=[],rhand=[0,])
setGain(taskWaist.gain,(2,0.2,0.03,0.9))
taskWaist.feature.selec.value = '011100'
def graspIf():
if linalg.norm(array(taskRH.task.error.value))<1e-2:
print grasp.__doc__
grasp()
@optionalparentheses
def now(t=None):
if t==None: t=robot.state.time
attime(t+1,clearAttimePeriodic)
attime(t+50,goForTheBall)
#attime(t+500,grasp)
now(3250)
# ------------------------------------------------------------------------------
from math import atan2,pi
def potFieldPG(target,pos,gain,obstacles):
''' Compute the velocity of the PG as the gradient of a potential field in
the 2D plan.'''
e = target-pos
waRwo = (array(dyn.waist.value)[0:2,0:2]).T
wo_vref = gain*e
wa_vref = dot(waRwo , wo_vref)
vmax = 0.2
vmax = array([0.3,0.1])
if abs(wa_vref[0])>vmax[0]: wa_vref *= vmax[0]/abs(wa_vref[0])
if abs(wa_vref[1])>vmax[1]: wa_vref *= vmax[1]/abs(wa_vref[1])
return wa_vref
def pgv():
t = robot.state.time
if t%50!=1: return
ball = array(taskRH.ref)[0:2,3]
v = potFieldPG( ball, array(dyn.com.value)[0:2],array((1,1)), [])
dth = atan2(v[1],v[0])
vref = (v[0],v[1], 0.2*dth )
pg.pg.velocitydes.value = vref
attime(ALWAYS,pgv)
#next()
go()
python/2013_coursens/p124_sing.py 0 → 100644
View file @ 60cf95b2
# ______________________________________________________________________________
# ******************************************************************************
# ______________________________________________________________________________
# ******************************************************************************
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.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.meta_tasks_kine import *
from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
from dynamic_graph.sot.core.meta_task_visual_point import MetaTaskVisualPoint
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
from dynamic_graph.sot.core.utils.attime import attime,ALWAYS,refset,sigset
from numpy import *
from dynamic_graph.sot.core.utils.history import History
from dynamic_graph.sot.dyninv.robot_specific 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 dynamic_graph.sot.dyninv.robot_specific import halfSittingConfig
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)
robot.set( initialConfig[robotName] )
addRobotViewer(robot,small=True,verbose=True)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
robot.increment(dt)
attime.run(robot.control.time)
updateComDisplay(robot,dyn.com)
history.record()
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()
# ---- 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
def toList(sot):
return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
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)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist')
taskLH.opmodif = matrixToTuple(handMgrip)
taskLH.feature.frame('desired')
# --- STATIC COM (if not walking)
taskCom = MetaTaskKineCom(dyn)
# --- TASK AVOID
taskShoulder=MetaTaskKine6d('shoulder',dyn,'shoulder','LARM_JOINT0')
taskShoulder.feature.frame('desired')
gotoNd(taskShoulder,(0,0,0),'010')
taskShoulder.task = TaskInequality('taskShoulderAvoid')
taskShoulder.task.add(taskShoulder.feature.name)
taskShoulder.task.referenceInf.value = (-10,) # Xmin, Ymin
taskShoulder.task.referenceSup.value = (0.20,) # Xmin, Ymin
taskShoulder.task.dt.value=dt
taskShoulder.task.controlGain.value = 0.9
taskElbow=MetaTaskKine6d('elbow',dyn,'elbow','LARM_JOINT3')
taskElbow.feature.frame('desired')
gotoNd(taskElbow,(0,0,0),'010')
taskElbow.task = TaskInequality('taskElbowAvoid')
taskElbow.task.add(taskElbow.feature.name)
taskElbow.task.referenceInf.value = (-10,) # Xmin, Ymin
taskElbow.task.referenceSup.value = (0.20,) # Xmin, Ymin
taskElbow.task.dt.value=dt
taskElbow.task.controlGain.value = 0.9
# --- 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.15,0) # Xmin, Ymin
taskSupport.referenceSup.value = (0.11,0.15,0) # Xmax, Ymax
taskSupport.dt.value=dt
# --- POSTURE ---
taskPosture = MetaTaskKinePosture(dyn)
# --- GAZE ---
taskGaze = MetaTaskVisualPoint('gaze',dyn,'head','gaze')
# Head to camera matrix transform
# Camera RU headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]])
# Camera LL
headMcam=array([[0.0,0.0,1.0,0.081],[1.,0.0,0.0,0.072],[0.0,1.,0.0,0.031],[0.0,0.0,0.0,1.0]])
headMcam = dot(headMcam,rotate('x',10*pi/180))
taskGaze.opmodif = matrixToTuple(headMcam)
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,dyn,name,joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
locals()['contact'+name] = contact
# --- TRACER -----------------------------------------------------------------
from dynamic_graph.tracer import *
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
tr.start()
robot.after.addSignal('tr.triger')
tr.add('dyn.com','com')
history = History(dyn,1)
# --- SHORTCUTS ----------------------------------------------------------------
def p6d(R,t):
M=eye(4)
M[0:3,0:3]=R
M[0:3,3]=t
return M
def push(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName not in sot.toList():
sot.push(taskName)
if taskName!="taskposture" and "taskposture" in sot.toList():
sot.down("taskposture")
def pop(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName in sot.toList(): sot.rm(taskName)
# --- DISPLAY ------------------------------------------------------------------
# --- DISPLAY ------------------------------------------------------------------
# --- DISPLAY ------------------------------------------------------------------
RAD=pi/180
comproj = [0.1,-0.95,1.6]
#robot.viewer.updateElementConfig('footproj',[0.5,0.15,1.6+0.08,0,-pi/2,0 ])
robot.viewer.updateElementConfig('footproj',comproj+[0,-pi/2,0 ])
robot.viewer.updateElementConfig('zmp2',[0,0,-10,0,0,0])
class BallPosition:
def __init__(self,xyz=(0,-1.1,0.9)):
self.ball = xyz
self.prec = 0
self.t = 0
self.duration = 0
self.f = 0
self.xyz= self.ball
def move(self,xyz,duration=50):
self.prec = self.ball
self.ball = xyz
self.t = 0
self.duration = duration
self.changeTargets()
if duration>0:
self.f = lambda : self.moveDisplay()
attime(ALWAYS,self.f)
else:
self.moveDisplay()
def moveDisplay(self):
tau = 1.0 if self.duration<=0 else float(self.t) / self.duration
xyz = tau * array(self.ball) + (1-tau) * array(self.prec)
robot.viewer.updateElementConfig('zmp',vectorToTuple(xyz)+(0,0,0))
self.t += 1
if self.t>self.duration and self.duration>0:
attime.stop(self.f)
self.xyz= xyz
def changeTargets(self):
gotoNd(taskRH,self.ball,'111',(4.9,0.9,0.01,0.9))
b = BallPosition()
tr.add('dyn.com','com')
tr.add(taskRH.task.name+'.error','erh')
tr.add(taskSupport.name+".normalizedPosition",'comn')
robot.after.addSignal(taskSupport.name+".normalizedPosition")
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
dyn.com.recompute(0)
taskCom.featureDes.errorIN.value = dyn.com.value
taskCom.task.controlGain.value = 10
taskPosture.gotoq(1,halfSittingConfig[robotName],chest=[],head=[],rleg=[],lleg=[],rarm=[],larm=[])
ball = BallPosition((0,-1.1,0.9))
#sot.damping.value = 0.1
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskRH)
ball.move((1.2,-0.4,1),0)
next()
next()
next()
def stop570():
'''Singularity reached, COM disable, press go to continue.'''
stop()
next()
next()
next()
pop(taskCom)
uinp = raw_input(' Enable COM regulation [Y/n]? >> ')
if uinp!='n':
push(taskCom)
attime(570,stop570)
go()
python/2013_coursens/p96_sing.py 0 → 100644
View file @ 60cf95b2
# ______________________________________________________________________________
# ******************************************************************************
# ______________________________________________________________________________
# ******************************************************************************
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.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.meta_tasks_kine import *
from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
from dynamic_graph.sot.core.meta_task_visual_point import MetaTaskVisualPoint
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
from dynamic_graph.sot.core.utils.attime import attime,ALWAYS,refset,sigset
from numpy import *
from dynamic_graph.sot.core.utils.history import History
from dynamic_graph.sot.dyninv.robot_specific 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 dynamic_graph.sot.dyninv.robot_specific import halfSittingConfig
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)
robot.set( initialConfig[robotName] )
addRobotViewer(robot,small=True,verbose=True)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
robot.increment(dt)
attime.run(robot.control.time)
updateComDisplay(robot,dyn.com)
history.record()
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()
# ---- 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
def toList(sot):
return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
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)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist')
taskLH.opmodif = matrixToTuple(handMgrip)
taskLH.feature.frame('desired')
# --- STATIC COM (if not walking)
taskCom = MetaTaskKineCom(dyn)
# --- TASK AVOID
taskShoulder=MetaTaskKine6d('shoulder',dyn,'shoulder','LARM_JOINT0')
taskShoulder.feature.frame('desired')
gotoNd(taskShoulder,(0,0,0),'010')
taskShoulder.task = TaskInequality('taskShoulderAvoid')
taskShoulder.task.add(taskShoulder.feature.name)
taskShoulder.task.referenceInf.value = (-10,) # Xmin, Ymin
taskShoulder.task.referenceSup.value = (0.20,) # Xmin, Ymin
taskShoulder.task.dt.value=dt
taskShoulder.task.controlGain.value = 0.9
taskElbow=MetaTaskKine6d('elbow',dyn,'elbow','LARM_JOINT3')
taskElbow.feature.frame('desired')
gotoNd(taskElbow,(0,0,0),'010')
taskElbow.task = TaskInequality('taskElbowAvoid')
taskElbow.task.add(taskElbow.feature.name)
taskElbow.task.referenceInf.value = (-10,) # Xmin, Ymin
taskElbow.task.referenceSup.value = (0.20,) # Xmin, Ymin
taskElbow.task.dt.value=dt
taskElbow.task.controlGain.value = 0.9
# --- 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.15,0) # Xmin, Ymin
taskSupport.referenceSup.value = (0.11,0.15,0) # Xmax, Ymax
taskSupport.dt.value=dt
# --- POSTURE ---
taskPosture = MetaTaskKinePosture(dyn)
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,dyn,name,joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
locals()['contact'+name] = contact
# --- TRACER -----------------------------------------------------------------
from dynamic_graph.tracer import *
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
tr.start()
robot.after.addSignal('tr.triger')
tr.add('dyn.com','com')
history = History(dyn,1)
# --- SHORTCUTS ----------------------------------------------------------------
def p6d(R,t):
M=eye(4)
M[0:3,0:3]=R
M[0:3,3]=t
return M
def push(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName not in sot.toList():
sot.push(taskName)
if taskName!="taskposture" and "taskposture" in sot.toList():
sot.down("taskposture")
def pop(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName in sot.toList(): sot.rm(taskName)
# --- DISPLAY ------------------------------------------------------------------
# --- DISPLAY ------------------------------------------------------------------
# --- DISPLAY ------------------------------------------------------------------
RAD=pi/180
comproj = [0.1,-0.95,1.6]
#robot.viewer.updateElementConfig('footproj',[0.5,0.15,1.6+0.08,0,-pi/2,0 ])
robot.viewer.updateElementConfig('footproj',comproj+[0,-pi/2,0 ])
robot.viewer.updateElementConfig('zmp2',[0,0,-10,0,0,0])
class BallPosition:
def __init__(self,xyz=(0,-1.1,0.9)):
self.ball = xyz
self.prec = 0
self.t = 0
self.duration = 0
self.f = 0
self.xyz= self.ball
def move(self,xyz,duration=50):
self.prec = self.ball
self.ball = xyz
self.t = 0
self.duration = duration
self.changeTargets()
if duration>0:
self.f = lambda : self.moveDisplay()
attime(ALWAYS,self.f)
else:
self.moveDisplay()
def moveDisplay(self):
tau = 1.0 if self.duration<=0 else float(self.t) / self.duration
xyz = tau * array(self.ball) + (1-tau) * array(self.prec)
robot.viewer.updateElementConfig('zmp',vectorToTuple(xyz)+(0,0,0))
self.t += 1
if self.t>self.duration and self.duration>0:
attime.stop(self.f)
self.xyz= xyz
def changeTargets(self):
gotoNd(taskRH,self.ball,'111',(4.9,0.9,0.01,0.9))
b = BallPosition()
tr.add('dyn.com','com')
tr.add(taskRH.task.name+'.error','erh')
tr.add(taskSupport.name+".normalizedPosition",'comn')
robot.after.addSignal(taskSupport.name+".normalizedPosition")
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
dyn.com.recompute(0)
taskCom.featureDes.errorIN.value = dyn.com.value
taskCom.task.controlGain.value = 10
taskPosture.gotoq(1,halfSittingConfig[robotName],chest=[],head=[],rleg=[],lleg=[],rarm=[],larm=[])
ball = BallPosition((0,-1.1,0.9))
sot.damping.value = 0.1
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskCom)
push(taskRH)
push(taskPosture)
ball.move((0.5,-0.4,1.2),0)
next()
next()
next()
q0=(-0.013448627761452581, -0.0024679718155040885, 0.63088361991552966, -0.018777584993745728, -0.060941237730978988, 0.037292111609228754, -0.037510492296196345, 0.021074469596564682, -0.45832797319827417, 0.99704625676400405, -0.47711515439441948, -0.0045892937844194013, -0.0375232834669115, 0.021281798238735231, -0.47292880929139702, 1.00958227703776, -0.47505006770217456, -0.0047970164517906902, 0.072094769948381279, 0.011059478621976536, -0.00011090786283310123, 0.0010171428665263665, -0.52826992474554724, -0.45608428199514117, 0.042817559290084496, -1.2032584278571674, 0.18143720449535519, -0.89040683552343491, -0.029527723367927538, 0.24934327906079795, 0.15262968598049148, -0.0042541976395581915, -0.52055386436392925, 4.8770209960872496e-05, 0.00060030850701126319, 0.57928762243102871)
ball.move((0.5,-0.2,1.2),0)
taskPosture.gotoq(1,tuple(q0),chest=[],head=[],rleg=[],lleg=[],rarm=[],larm=[])
class Swing:
def __init__(self):
self.w=1.0/200.0*5.0
self.th=30*RAD
self.first=False
self.Kdot = 0.1/200/0.2
def __call__(self):
t=robot.control.time
q=list(robot.state.value)
if not self.first:
self.t0=t
self.q0=q
self.th=self.q0[23]
self.first=True
q[23] = self.th*cos(self.w*(t-self.t0))
taskPosture.gotoq(10,tuple(q),chest=[],head=[],rleg=[],lleg=[],rarm=[],larm=[])
self.th+=self.Kdot*self.th
def start(self):
robot.displayList.append(swing)
swing=Swing()
@optionalparentheses
def n():swing.start()
print '\n\n\nPress n to relax the elbow.'''
go()
python/2013_coursens/xp2_kineana.py 0 → 100644
View file @ 60cf95b2
# ______________________________________________________________________________
# ******************************************************************************
# ______________________________________________________________________________
# ******************************************************************************
import sys
execfile('/home/nmansard/.pythonrc')
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 dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.meta_tasks_kine import *
from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
from dynamic_graph.sot.core.meta_task_visual_point import MetaTaskVisualPoint
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
from dynamic_graph.sot.core.utils.attime import attime,ALWAYS,refset,sigset
from numpy import *
from dynamic_graph.sot.core.utils.history import History
from dynamic_graph.sot.dyninv.robot_specific import pkgDataRootDir,modelName,robotDimension,initialConfig
from dynamic_graph.sot.dyninv.robot_specific import gearRatio,inertiaRotor,halfSittingConfig
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
robotName = 'hrp14small'
robotDim=robotDimension[robotName]
robot = RobotSimu("robot")
robot.resize(robotDim)
dt=5e-3
x0=-0.00949035111398315034
y0=0
z0=0.64870185118253043 #0.6487018512
halfSittingConfig[robotName] = (x0,y0,z0,0,0,0)+halfSittingConfig[robotName][6:]
q0=list(halfSittingConfig[robotName])
initialConfig[robotName]=tuple(q0)
q0[20]=-1
# Object is centered
#q0 = (-0.0270855404604077, 0.0058984313447739307, 0.64596879258040218, 0.051646911457782906, -0.01849697876637613, -0.26694316842988736, 0.26764629500934506, -0.059954849690319403, -0.49830573060840277, 0.90083178777804507, -0.37102964759029244, 0.01502407314938717, 0.26738768265871021, -0.051757611229943074, -0.42500685369248403, 0.87129911754613565, -0.41481029199661462, 0.0068227689907173691, -0.57713162476592372, 0.14306500463299326, -0.78539800046042108, 0.23559202790602107, 0.25461821795561479, -0.1618842307250519, 0.0026405447564797208, -0.5275700723685236, 2.0836791732190813e-05, -0.00077505993604308769, 0.21347640028265377, 0.25464098548316366, 0.18983684206548987, 0.0028852330073878755, -0.52655196479787658, -1.5442864197634829e-05, -0.00056592322550752386, 0.13166002981175504)
#initialConfig[robotName]=tuple(q0)
robot.set( initialConfig[robotName] )
addRobotViewer(robot,small=True,verbose=True)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
robot.increment(dt)
attime.run(robot.control.time)
updateComDisplay(robot,dyn.com)
history.record()
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
def toList(sot):
return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
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)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist')
taskLH.opmodif = matrixToTuple(handMgrip)
taskLH.feature.frame('desired')
# --- STATIC COM (if not walking)
taskCom = MetaTaskKineCom(dyn)
# --- TASK AVOID
taskShoulder=MetaTaskKine6d('shoulder',dyn,'shoulder','LARM_JOINT0')
taskShoulder.feature.frame('desired')
gotoNd(taskShoulder,(0,0,0),'010')
taskShoulder.task = TaskInequality('taskShoulderAvoid')
taskShoulder.task.add(taskShoulder.feature.name)
taskShoulder.task.referenceInf.value = (-10,) # Xmin, Ymin
taskShoulder.task.referenceSup.value = (0.20,) # Xmin, Ymin
taskShoulder.task.dt.value=dt
taskShoulder.task.controlGain.value = 0.9
taskElbow=MetaTaskKine6d('elbow',dyn,'elbow','LARM_JOINT3')
taskElbow.feature.frame('desired')
gotoNd(taskElbow,(0,0,0),'010')
taskElbow.task = TaskInequality('taskElbowAvoid')
taskElbow.task.add(taskElbow.feature.name)
taskElbow.task.referenceInf.value = (-10,) # Xmin, Ymin
taskElbow.task.referenceSup.value = (0.20,) # Xmin, Ymin
taskElbow.task.dt.value=dt
taskElbow.task.controlGain.value = 0.9
# --- 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.15,0) # Xmin, Ymin
taskSupport.referenceSup.value = (0.11,0.15,0) # Xmax, Ymax
taskSupport.dt.value=dt
# --- TASK SUPPORT SMALL --------------------------------------------
featureSupportSmall = FeatureGeneric('featureSupportSmall')
plug(dyn.com,featureSupportSmall.errorIN)
plug(dyn.Jcom,featureSupportSmall.jacobianIN)
taskSupportSmall=TaskInequality('taskSupportSmall')
taskSupportSmall.add(featureSupportSmall.name)
taskSupportSmall.selec.value = '011'
#taskSupportSmall.referenceInf.value = (-0.02,-0.02,0) # Xmin, Ymin
#askSupportSmall.referenceSup.value = (0.02,0.02,0) # Xmax, Ymax
taskSupportSmall.referenceInf.value = (-0.02,-0.05,0) # Xmin, Ymin
taskSupportSmall.referenceSup.value = (0.02,0.05,0) # Xmax, Ymax
taskSupportSmall.dt.value=dt
# --- POSTURE ---
taskPosture = MetaTaskKinePosture(dyn)
# --- GAZE ---
taskGaze = MetaTaskVisualPoint('gaze',dyn,'head','gaze')
# Head to camera matrix transform
# Camera RU headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]])
# Camera LL
headMcam=array([[0.0,0.0,1.0,0.081],[1.,0.0,0.0,0.072],[0.0,1.,0.0,0.031],[0.0,0.0,0.0,1.0]])
headMcam = dot(headMcam,rotate('x',10*pi/180))
taskGaze.opmodif = matrixToTuple(headMcam)
# --- FOV ---
taskFoV = MetaTaskVisualPoint('FoV',dyn,'head','gaze')
taskFoV.opmodif = matrixToTuple(headMcam)
taskFoV.task=TaskInequality('taskFoVineq')
taskFoV.task.add(taskFoV.feature.name)
[Xmax,Ymax]=[0.38,0.28]
taskFoV.task.referenceInf.value = (-Xmax,-Ymax) # Xmin, Ymin
taskFoV.task.referenceSup.value = (Xmax,Ymax) # Xmax, Ymax
taskFoV.task.dt.value=dt
taskFoV.task.controlGain.value=0.01
taskFoV.featureDes.xy.value = (0,0)
# --- ORIENTATION -------------------------------------------------------------
from dynamic_graph.sot.core.feature_vector3 import FeatureVector3
featureVectorRH = FeatureVector3('fv3rh')
plug(dyn.rh,featureVectorRH.position)
plug(dyn.Jrh,featureVectorRH.Jq)
featureVectorRH.vector.value = (1,0,0)
featureVectorRH.positionRef.value = (1,0,0)
taskVectorRH = Task('taskVectorRH')
taskVectorRH.add(featureVectorRH.name)
taskVectorRH.controlGain.value = 10
featureVectorLH = FeatureVector3('fv3lh')
plug(dyn.lh,featureVectorLH.position)
plug(dyn.Jlh,featureVectorLH.Jq)
featureVectorLH.vector.value = (1,0,0)
featureVectorLH.positionRef.value = (1,0,0)
taskVectorLH = Task('taskVectorLH')
taskVectorLH.add(featureVectorLH.name)
gainVectorLH = GainAdaptive("gainVectorLH")
plug(taskVectorLH.error,gainVectorLH.error)
plug(gainVectorLH.gain,taskVectorLH.controlGain)
setGain(gainVectorLH,(10,1.5,0.01,0.9))
# Control the orientation of the gripper toward the continuous handle.
taskRHOrient=MetaTaskKine6d('rhorient',dyn,'rh','right-wrist')
taskRHOrient.opmodif = matrixToTuple(handMgrip)
taskRHOrient.feature.frame('current')
taskRHOrient.feature.selec.value = '011'
taskRHOrient.featureVector = FeatureVector3('feature-rhorient-v3')
plug(taskRHOrient.feature.position,taskRHOrient.featureVector.position)
plug(taskRHOrient.feature.Jq,taskRHOrient.featureVector.Jq)
taskRHOrient.featureVector.vector.value = (1,0,0)
taskRHOrient.task.add(taskRHOrient.featureVector.name)
setGain(taskRHOrient.gain,1)
taskLHOrient=MetaTaskKine6d('lhorient',dyn,'lh','left-wrist')
taskLHOrient.opmodif = matrixToTuple(handMgrip)
taskLHOrient.feature.frame('current')
taskLHOrient.feature.selec.value = '011'
taskLHOrient.featureVector = FeatureVector3('feature-lhorient-v3')
plug(taskLHOrient.feature.position,taskLHOrient.featureVector.position)
plug(taskLHOrient.feature.Jq,taskLHOrient.featureVector.Jq)
taskLHOrient.featureVector.vector.value = (1,0,0)
taskLHOrient.task.add(taskLHOrient.featureVector.name)
setGain(taskLHOrient.gain,1)
# --- Task Joint Limits -----------------------------------------
dyn.upperJl.recompute(0)
dyn.lowerJl.recompute(0)
taskJL = TaskJointLimits('taskJL')
plug(dyn.position,taskJL.position)
taskJL.controlGain.value = 10
taskJL.referenceInf.value = dyn.lowerJl.value
taskJL.referenceSup.value = dyn.upperJl.value
taskJL.dt.value = dt
taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
# ---- WAIST TASK ---
taskWaist=MetaTask6d('waist',dyn,'waist','waist')
pg.plugWaistTask(taskWaist)
taskWaist.task.controlGain.value = 5
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,dyn,name,joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
locals()['contact'+name] = contact
# --- TRACER -----------------------------------------------------------------
from dynamic_graph.tracer import *
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
tr.start()
robot.after.addSignal('tr.triger')
tr.add('dyn.com','com')
history = History(dyn,1)
# --- SHORTCUTS ----------------------------------------------------------------
qn = taskJL.normalizedPosition
@optionalparentheses
def pqn(details=True):
''' Display the normalized configuration vector. '''
qn.recompute(robot.state.time)
s = [ "{0:.1f}".format(v) for v in qn.value]
if details:
print("Rleg: "+" ".join(s[:6]))
print("Lleg: "+" ".join(s[6:12]))
print("Body: "+" ".join(s[12:16]))
print("Rarm: "+" ".join(s[16:23]))
print("Larm :"+" ".join(s[23:30]))
else:
print(" ".join(s[:30]))
def jlbound(t=None):
'''Display the velocity bound induced by the JL as a double-column matrix.'''
if t==None: t=robot.state.time
taskJL.task.recompute(t)
return matrix([ [float(x),float(y)] for x,y
in [ c.split(',') for c in taskJL.task.value[6:-3].split('),(') ] ])
def p6d(R,t):
M=eye(4)
M[0:3,0:3]=R
M[0:3,3]=t
return M
def push(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName not in sot.toList():
sot.push(taskName)
if taskName!="taskposture" and "taskposture" in sot.toList():
sot.down("taskposture")
def pop(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName in sot.toList(): sot.rm(taskName)
# --- DISPLAY ------------------------------------------------------------------
# --- DISPLAY ------------------------------------------------------------------
# --- DISPLAY ------------------------------------------------------------------
RAD=pi/180
comproj = [0.1,-0.95,1.6]
#robot.viewer.updateElementConfig('footproj',[0.5,0.15,1.6+0.08,0,-pi/2,0 ])
robot.viewer.updateElementConfig('footproj',comproj+[0,-pi/2,0 ])
robot.viewer.updateElementConfig('zmp2',[0,0,-10,0,0,0])
def updateComProj(com=None):
if com==None:
[x,y,z]=com=dyn.com.value
else:
[x,y,z]=com
p=array(comproj)+[0,y,-x]
robot.viewer.updateElementConfig('zmp2',vectorToTuple(p)+(0,0,0 ))
robot.displayList.append( updateComProj)
def displayFov(recompute=False):
if recompute: taskGaze.proj.transfo.recompute(robot.state.time)
robot.viewer.updateElementConfig('fov',matrixToRPY(taskGaze.proj.transfo.value))
displayFov(True)
robot.after.addSignal(taskGaze.proj.name+'.transfo')
robot.displayList.append(displayFov)
class BallPosition:
def __init__(self,xyz=(0,-1.1,0.9)):
self.ball = xyz
self.prec = 0
self.t = 0
self.duration = 0
self.f = 0
self.xyz= self.ball
def move(self,xyz,duration=50):
self.prec = self.ball
self.ball = xyz
self.t = 0
self.duration = duration
self.changeTargets()
if duration>0:
self.f = lambda : self.moveDisplay()
attime(ALWAYS,self.f)
else:
self.moveDisplay()
def moveDisplay(self):
tau = 1.0 if self.duration<=0 else float(self.t) / self.duration
xyz = tau * array(self.ball) + (1-tau) * array(self.prec)
robot.viewer.updateElementConfig('zmp',vectorToTuple(xyz)+(0,0,0))
self.t += 1
if self.t>self.duration and self.duration>0:
attime.stop(self.f)
self.xyz= xyz
def changeTargets(self):
gotoNd(taskRH,self.ball,'111',(4.9,0.9,0.01,0.9))
taskFoV.goto3D(self.ball)
b = BallPosition()
tr.add(taskJL.name+".normalizedPosition","qn")
tr.add('dyn.com','com')
tr.add(taskRH.task.name+'.error','erh')
tr.add(taskFoV.feature.name+'.error','fov')
tr.add(taskFoV.task.name+'.normalizedPosition','fovn')
tr.add(taskSupportSmall.name+".normalizedPosition",'comsmalln')
tr.add(taskSupport.name+".normalizedPosition",'comn')
robot.after.addSignal(taskJL.name+".normalizedPosition")
robot.after.addSignal(taskSupportSmall.name+".normalizedPosition")
robot.after.addSignal(taskFoV.task.name+".normalizedPosition")
robot.after.addSignal(taskFoV.feature.name+'.error')
robot.after.addSignal(taskSupport.name+".normalizedPosition")
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
taskCom.featureDes.errorIN.value = dyn.com.value
taskCom.task.controlGain.value = 10
taskPosture.gotoq(1,array(q0),chest=[],head=[],rleg=[],lleg=[],rarm=[],larm=[])
sot.push(taskJL.name)
sot.addContact(contactRF)
sot.addContact(contactLF)
sot.push(taskSupport.name)
ball = BallPosition((0,-1.1,0.9))
tw=TaskWeight('tw')
'''
taskFoV.task.controlGain.value=0.05
taskSupportSmall.controlGain.value=0.02
tw.add(taskSupportSmall.name,1)
tw.add(taskFoV.task.name,10)
sot.damping.value = 0.01
'''
taskFoV.task.controlGain.value=0.05
taskSupportSmall.controlGain.value=0.02
tw.add(taskSupportSmall.name,1)
tw.add(taskFoV.task.name,3)
sot.damping.value = 0.01
#sot.damping.value = 0.6
#ball.move((0.1,-1.2,0.9))
push(taskRH)
push(tw)
ball.move((0.5,-0.2,1.3),0)
attime(400,lambda:ball.move((1,0.5,0.9)))
attime(1000,lambda: ball.move((0.6,0.,0.8)))
attime(1400,lambda: ball.move((0.1,-1.2,0.9)))
attime(1401,lambda: sigset(sot.damping,0.1))
attime(1900,lambda: sigset(sot.damping,0.9))
attime(2080,stop)
#for t in [400,1000,1400]: attime(t,stop)
#import time
#for t in [1000]: attime(t,lambda: time.sleep(1))
#time.sleep(2)
for t in filter(lambda x: isinstance(x,int),attime.events.keys()): attime(t,lambda:sot.resetAset() )
print filter(lambda x: isinstance(x,int),attime.events.keys())
# --- DG ---
'''
taskSupportSmall.controlGain.value=0.1
taskJL.controlGain.value = 1
taskSupport.controlGain.value = 1
del attime.events[1900]
del attime.events[1401]
attime(1800,lambda: sigset(sot.damping,0.9))
attime(1920,lambda: sigset(sot.damping,0.1))
attime(1920, lambda: pop(tw))
attime(850,lambda: sigset(taskSupportSmall.controlGain,0.01))
'''
go()
python/CMakeLists.txt 0 → 100644
View file @ 60cf95b2
IF(BUILD_PYTHON_INTERFACE)
CONFIGURE_FILE(
"${CMAKE_CURRENT_SOURCE_DIR}/robot_specific.py.cmake"
"${CMAKE_CURRENT_BINARY_DIR}/robot_specific.py"
)
INSTALL(FILES
${CMAKE_CURRENT_BINARY_DIR}/robot_specific.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/dyninv
)
INSTALL(
FILES ros/sot-concept.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/tutorial
)
ENDIF(BUILD_PYTHON_INTERFACE)
python/jumble/dyndebug.py 0 → 100644
View file @ 60cf95b2
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 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 ):
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]
RobotClass = RobotDynSimu
robot = RobotClass("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]
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()
except:
print "No robot viewer, sorry."
robot.viewer = None
# --- MAIN LOOP ------------------------------------------
qs=[]
def inc():
robot.increment(dt)
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(): inc() #runner.once()
# --- shortcuts -------------------------------------------------
@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 ----------------------------------------------------------------------
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)
plug(robot.velocity,dyn.velocity)
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 -----------------------------------------
# Task right hand
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=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 ------------------------------------------------------
dyn.setProperty('ComputeCoM','true')
featureCom = FeatureGeneric('featureCom')
featureComDes = FeatureGeneric('featureComDes')
plug(dyn.com,featureCom.errorIN)
plug(dyn.Jcom,featureCom.jacobianIN)
featureCom.setReference('featureComDes')
featureComDes.errorIN.value = (0.0478408688115,-0.0620357207995,0.684865189311)
taskCom = TaskDynPD('taskCom')
taskCom.add('featureCom')
plug(dyn.velocity,taskCom.qdot)
taskCom.dt.value = 1e-3
gCom = GainAdaptive('gCom')
plug(taskCom.error,gCom.error)
plug(gCom.gain,taskCom.controlGain)
# Work from .04 to .09 gCom.set 1050 45 125e3
# Good behavior gCom.set 1080 15 125e3
# Low gains gCom.set 400 1 39e3
# Current choice
gCom.set(1050,45,125e3)
# ---- CONTACT -----------------------------------------
# Left foot contact
contactLF = MetaTaskDyn6d('contact_lleg',dyn,'lf','left-ankle')
contactLF.support = ((0.11,-0.08,-0.08,0.11),(-0.045,-0.045,0.07,0.07),(-0.105,-0.105,-0.105,-0.105))
contactLF.feature.frame('desired')
# Right foot contact
contactRF = MetaTaskDyn6d('contact_rleg',dyn,'rf','right-ankle')
contactRF.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))
contactRF.feature.frame('desired')
# --- SOT Dyn OpSpaceH --------------------------------------
# SOT controller.
sot = SolverOpSpace('sot')
sot.setSize(robotDim-6)
#sot.damping.value = 2e-2
sot.breakFactor.value = 10
plug(dyn.inertiaReal,sot.matrixInertia)
plug(dyn.dynamicDrift,sot.dyndrift)
plug(dyn.velocity,sot.velocity)
plug(sot.control,robot.control)
# For the integration of q = int(int(qddot)).
plug(sot.acceleration,robot.acceleration)
# --- RUN ------------------------------------------------
featureComDes.errorIN.value = (0.06,0,0.8)
sot.addContactFromTask(contactLF.task.name,'LF')
sot._LF_p.value = contactLF.support
sot.addContactFromTask(contactRF.task.name,'RF')
sot._RF_p.value = contactRF.support
sot.push('taskCom')
#go
python/jumble/dynpg.py 0 → 100644
View file @ 60cf95b2
#_____________________________________________________________________________________________
#********************************************************************************************
#
# Robot motion (HRP2 14 small) using:
# - ONLY OPERATIONAL TASKS
# - Joint limits (position and velocity)
#_____________________________________________________________________________________________
#*********************************************************************************************
import sys
from optparse import OptionParser
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
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 numpy import *
from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
from matrix_util import matrixToTuple, vectorToTuple,rotate
from history import History
from zmp_estimator import ZmpEstimator
from viewer_helper import addRobotViewer,VisualPinger
#-----------------------------------------------------------------------------
# --- ROBOT SIMU -------------------------------------------------------------
#-----------------------------------------------------------------------------
robotName = 'hrp14small'
robotDim = robotDimension[robotName]
RobotClass = RobotDynSimu
robot = RobotClass("robot")
robot.resize(robotDim)
addRobotViewer(robot,small=True,verbose=False)
dt = 5e-3
# Similar initial position with hand forward
initialConfig['hrp14small'] = (0,0,0.648697398115,0,0,0)+(0, 0, -0.4538, 0.8727, -0.4189, 0, 0, 0, -0.4538, 0.8727, -0.4189, 0, 0, 0, 0, 0, 0.2618, -0.1745, 0, -0.5236, 0, 0, 0, 0.2618, 0.1745, 0, -0.5236, 0, 0, 0 )
robot.set(initialConfig[robotName])
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
def inc():
robot.increment(dt)
# Execute a function at time t, if specified with t.add(...)
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])
if dyn.com.time >0:
robot.viewer.updateElementConfig('com',[dyn.com.value[0],dyn.com.value[1],0,0,0,0])
history.record()
from ThreadInterruptibleLoop import *
@loopInThread
def loop():
# try:
inc()
# except:
# print robot.state.time,': -- Robot has stopped --'
runner=loop()
@optionalparentheses
def go(): runner.play()
@optionalparentheses
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.rightSupport()) ).resstr
@optionalparentheses
def dump(): history.dumpToOpenHRP('openhrp/screenwash')
#-----------------------------------------------------------------------------
#---- 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.lowerJl.recompute(0)
dyn.upperJl.recompute(0)
llimit = matrix(dyn.lowerJl.value)
ulimit = matrix(dyn.upperJl.value)
dlimit = ulimit-llimit
mlimit = (ulimit+llimit)/2
llimit[6:18] = mlimit[6:12] - dlimit[6:12]*0.48
dyn.upperJl.value = vectorToTuple(ulimit)
dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value = gearRatio[robotName]
plug(robot.state,dyn.position)
plug(robot.velocity,dyn.velocity)
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 ---------------------------------------------------------
from dynamic_graph.sot.pattern_generator import *
pg = PatternGenerator('pg')
pg.setVrmlDir(modelDir+'/')
pg.setVrml(modelName[robotName])
pg.setXmlSpec(specificitiesPath)
pg.setXmlRank(jointRankPath)
pg.buildModel()
# Standard initialization
pg.parseCmd(":samplingperiod 0.005")
pg.parseCmd(":previewcontroltime 1.6")
pg.parseCmd(":comheight 0.814")
pg.parseCmd(":omega 0.0")
pg.parseCmd(":stepheight 0.05")
pg.parseCmd(":singlesupporttime 0.780")
pg.parseCmd(":doublesupporttime 0.020")
pg.parseCmd(":armparameters 0.5")
pg.parseCmd(":LimitsFeasibility 0.0")
pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015")
pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.0 3.0")
pg.parseCmd(":UpperBodyMotionParameters 0.0 -0.5 0.0")
pg.parseCmd(":comheight 0.814")
pg.parseCmd(":SetAlgoForZmpTrajectory Morisawa")
plug(dyn.position,pg.position)
plug(dyn.com,pg.com)
pg.motorcontrol.value = robotDim*(0,)
pg.zmppreviouscontroller.value = (0,0,0)
pg.initState()
# --- REFERENCES ---------------------------------------------------------------
# --- Selector of Com Ref: when pg is stopped, pg.inprocess becomes 0
comRef = Selector('comRef'
,['vector','ref',dyn.com,pg.comref])
plug(pg.inprocess,comRef.selec)
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.parseCmd(':SetAlgoForZmpTrajectory Herdt')
pg.parseCmd(':doublesupporttime 0.1')
pg.parseCmd(':singlesupporttime 0.7')
# When velocity reference is at zero, the robot stops all motion after n steps
pg.parseCmd(':numberstepsbeforestop 4')
# Set constraints on XY
pg.parseCmd(':setfeetconstraint XY 0.09 0.06')
# Start the robot with a speed of 0.1 m/0.8 s.
pg.parseCmd(':HerdtOnline 0.1 0.0 0.0')
# You can now modifiy the speed of the robot using set pg.velocitydes [3]( x, y, yaw)
pg.velocitydes.value =(0.1,0.0,0.0)
#-----------------------------------------------------------------------------
# --- OPERATIONAL TASKS (For HRP2-14)---------------------------------------------
#-----------------------------------------------------------------------------
# --- Op task for the waist ------------------------------
taskWaist = MetaTaskDyn6d('taskWaist',dyn,'waist','waist')
taskChest = MetaTaskDyn6d('taskChest',dyn,'chest','chest')
taskHead = MetaTaskDyn6d('taskHead',dyn,'head','gaze')
taskRH = MetaTaskDyn6d('rh',dyn,'rh','right-wrist')
taskLH = MetaTaskDyn6d('lh',dyn,'lh','left-wrist')
taskRF = MetaTaskDyn6d('rf',dyn,'rf','right-ankle')
taskLF = MetaTaskDyn6d('lf',dyn,'lf','left-ankle')
for task in [ taskWaist, taskChest, taskHead, taskRH, taskLH, taskRF, taskLF ]:
task.feature.frame('current')
task.gain.setConstant(50)
task.task.dt.value = dt
# --- TASK COM ------------------------------------------------------
taskCom = MetaTaskDynCom(dyn,dt)
# --- TASK POSTURE --------------------------------------------------
taskPosture = MetaTaskDynPosture(dyn,dt)
# --- Task lim ---------------------------------------------------
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])
#-----------------------------------------------------------------------------
# --- SOT Dyn OpSpaceH: SOT controller --------------------------------------
#-----------------------------------------------------------------------------
sot = SolverDynReduced('sot')
contact = AddContactHelper(sot)
sot.setSize(robotDim-6)
#sot.damping.value = 1e-2
sot.breakFactor.value = 10
plug(dyn.inertiaReal,sot.matrixInertia)
plug(dyn.dynamicDrift,sot.dyndrift)
plug(dyn.velocity,sot.velocity)
plug(sot.solution, robot.control)
#For the integration of q = int(int(qddot)).
plug(sot.acceleration,robot.acceleration)
#-----------------------------------------------------------------------------
# ---- CONTACT: Contact definition -------------------------------------------
#-----------------------------------------------------------------------------
# Left foot contact
contactLF = MetaTaskDyn6d('contact_lleg',dyn,'lf','left-ankle')
contactLF.feature.frame('desired')
contactLF.gain.setConstant(1000)
contactLF.name = "LF"
# Right foot contact
contactRF = MetaTaskDyn6d('contact_rleg',dyn,'rf','right-ankle')
contactRF.feature.frame('desired')
contactRF.gain.setConstant(1000)
contactRF.name = "RF"
contactRF.support = ((0.11,-0.08,-0.08,0.11),(-0.045,-0.045,0.07,0.07),(-0.105,-0.105,-0.105,-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))
#--- ZMP ---------------------------------------------------------------------
zmp = ZmpEstimator('zmp')
zmp.declare(sot,dyn)
#-----------------------------------------------------------------------------
# --- TRACE ------------------------------------------------------------------
#-----------------------------------------------------------------------------
from dynamic_graph.tracer import *
tr = Tracer('tr')
tr.open('/tmp/','pg_','.dat')
tr.add('dyn.com','com')
tr.add(taskCom.feature.name+'.error','ecom')
tr.add(taskCom.featureDes.name+'.errorIN','comref')
tr.add('dyn.waist','waist')
tr.add('dyn.rh','rh')
tr.add('zmp.zmp','')
tr.add('dyn.position','')
tr.add('dyn.velocity','')
tr.add('robot.acceleration','robot_acceleration')
tr.add('robot.control','')
tr.add(taskCom.gain.name+'.gain','com_gain')
tr.add(taskRF.gain.name+'.gain','rf_gain')
tr.add('dyn.lf','lf')
tr.add('dyn.rf','rf')
tr.add('pg.rightfootcontact','rfcontact')
tr.start()
robot.after.addSignal('tr.triger')
robot.after.addSignal(contactLF.task.name+'.error')
robot.after.addSignal('dyn.rf')
robot.after.addSignal('dyn.lf')
robot.after.addSignal('dyn.com')
robot.after.addSignal('sot.forcesNormal')
robot.after.addSignal('dyn.waist')
robot.after.addSignal('taskLim.normalizedPosition')
tr.add('taskLim.normalizedPosition','qn')
history = History(dyn,1,zmp.zmp)
#-----------------------------------------------------------------------------
# --- RUN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
# --- TASK WAIST ---
# Build the reference waist pos homo-matrix from PG.
waistReferenceVector = Stack_of_vector('waistReferenceVector')
plug(pg.initwaistposref,waistReferenceVector.sin1)
plug(pg.initwaistattref,waistReferenceVector.sin2)
waistReferenceVector.selec1(0,3)
waistReferenceVector.selec2(0,3)
waistReference=PoseRollPitchYawToMatrixHomo('waistReference')
plug(waistReferenceVector.sout,waistReference.sin)
plug(waistReference.sout,taskWaist.featureDes.position)
taskWaist.feature.selec.value = '011100'
taskWaist.task.controlGain.value = 1000
# --- TASK COM ---
plug(comRef.ref,taskCom.featureDes.errorIN)
plug(pg.dcomref,taskCom.featureDes.errordotIN)
taskCom.feature.selec.value = '011'
taskCom.gain.setConstant(10)
# --- TASKS FOOT ---
plug(pg.rightfootref,taskRF.featureDes.position)
taskRF.task.controlGain.value = 1000
plug(pg.leftfootref,taskLF.featureDes.position)
taskLF.task.controlGain.value = 1000
# --- SOT ---
sot.clear()
contact(contactLF)
contact(contactRF)
#sot.push(taskLim.name)
sot.push(taskCom.task.name)
plug(robot.state,sot.position)
sot.breakFactor.value = 2
# --- SELECTER ---
selec = ContactSelecter('contactSelec')
selec.setSolver(sot.name)
selec.setContactAndTask(contactRF.name,contactRF.task.name,taskRF.task.name)
selec.RFSupport.value = contactRF.support
plug(pg.rightfootcontact,selec.RFActivation)
selec.setContactStatus(contactRF.name,True)
selec.setContactAndTask(contactLF.name,contactLF.task.name,taskLF.task.name)
selec.LFSupport.value = contactLF.support
plug(pg.leftfootcontact,selec.LFActivation)
selec.setContactStatus(contactLF.name,True)
robot.before.addSignal(selec.name+'.trigger')
selec.verbose(True)
# --- Events ---------------------------------------------
sigset = ( lambda s,v : s.__class__.value.__set__(s,v) )
refset = ( lambda mt,v : mt.__class__.ref.__set__(mt,v) )
robot.before.addSignal(selec.name+'.trigger')
#for i in range(10): next()
#
go()
attime(646,stop)
python/jumble/dynreduced.py 0 → 100644
View file @ 60cf95b2
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 meta_task_dyn_6d import MetaTaskDyn6d
from attime import attime
from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
robotName = 'hrp10small'
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 ---------------------------------------------------------------
initialConfig['hrp14small'] = ( 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,
0.3, 0.3, 0, 0,
0.6, -0.3, 0, -0.52, 0, -0.1, 0,
-0.2, 0.17, 0, -0.52, 0, 0.1, 0 )
robotDim=robotDimension[robotName]
RobotClass = RobotDynSimu
robot = RobotClass("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]
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()
except:
#print "No robot viewer, sorry."
robot.viewer = None
# --- MAIN LOOP ------------------------------------------
#rs=True # Regular solver
rs=False # Reduced solver
if rs:
toBeRecomputed = ['matrixInertia','dyndrift']
else:
toBeRecomputed = ['Jc', 'driftContact','dyndrift', 'freeForceBase', 'freeMotionBase', 'inertiaSqrootInv']
qs=[]
chronos=[]
def inc():
tnext=robot.control.time+1
attime.run(tnext)
for sig in toBeRecomputed:
sot.signal(sig).recompute( tnext )
chrono=Chrono()
sot.triger.recompute(tnext)
chronos.append(chrono.tic())
robot.increment(dt)
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()
inc()
print sot.velocity.value
# --- shortcuts -------------------------------------------------
@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 ----------------------------------------------------------------------
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)
plug(robot.velocity,dyn.velocity)
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 -----------------------------------------
# Task right hand
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=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 ------------------------------------------------------
dyn.setProperty('ComputeCoM','true')
featureCom = FeatureGeneric('featureCom')
featureComDes = FeatureGeneric('featureComDes')
plug(dyn.com,featureCom.errorIN)
plug(dyn.Jcom,featureCom.jacobianIN)
featureCom.sdes.value = 'featureComDes'
taskCom = TaskDynPD('taskCom')
taskCom.add('featureCom')
plug(dyn.velocity,taskCom.qdot)
taskCom.dt.value = dt
gCom = GainAdaptive('gCom')
plug(taskCom.error,gCom.error)
plug(gCom.gain,taskCom.controlGain)
# Work from .04 to .09 gCom.set 1050 45 125e3
# Good behavior gCom.set 1080 15 125e3
# Low gains gCom.set 400 1 39e3
# Current choice
gCom.set(1050,45,125e3)
# ---- CONTACT -----------------------------------------
# Left foot contact
contactLF = MetaTaskDyn6d('contact_lleg',dyn,'lf','left-ankle')
contactLF.support = ((0.11,-0.08,-0.08,0.11),(-0.045,-0.045,0.07,0.07),(-0.105,-0.105,-0.105,-0.105))
contactLF.feature.frame('desired')
contactLF.task.dt.value=dt
# Right foot contact
contactRF = MetaTaskDyn6d('contact_rleg',dyn,'rf','right-ankle')
contactRF.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))
contactRF.feature.frame('desired')
contactRF.task.dt.value=dt
# --- SOT Dyn OpSpaceH --------------------------------------
# SOT controller.
if rs:
sot = SolverOpSpace('sot')
sot.triger = sot.control
else:
sot = SolverDynReduced('sot')
sot.triger = sot.acceleration
sot.setSize(robotDim-6)
#sot.damping.value = 2e-2
sot.breakFactor.value = 10
plug(dyn.inertiaReal,sot.matrixInertia)
plug(dyn.dynamicDrift,sot.dyndrift)
plug(dyn.velocity,sot.velocity)
if rs: plug(sot.control,robot.control)
else: plug(sot.solution,robot.control)
plug(sot.acceleration,robot.acceleration)
sot.addContactFromTask(contactLF.task.name,'LF')
sot._LF_p.value = contactLF.support
sot.addContactFromTask(contactRF.task.name,'RF')
sot._RF_p.value = contactRF.support
# --- TRACE ----------------------------------------------
from dynamic_graph.tracer import *
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
tr.start()
robot.after.addSignal('tr.triger')
tr.add('dyn.position','')
#tr.add('sot.forces','')
tr.add('sot.acceleration','')
tr.add('sot.velocity','')
#robot.after.addSignal('sot.forces')
robot.after.addSignal('sot.acceleration')
tr.add('taskCom.error','com')
tr.add('taskCom.task','')
tr.add(contactLF.task.name+'.error','lf')
tr.add(contactRF.task.name+'.error','rf')
if not rs: tr.add('sot.solution','')
if rs:
tr.add('sot._LF_f','fl')
tr.add('sot._RF_f','fr')
tr.add('sot._LF_fn','fnl')
tr.add('sot._RF_fn','fnr')
else:
tr.add('sot.forces','')
robot.after.addSignal('sot.forces')
if not rs:
tr.add('sot.forcesNormal','')
robot.after.addSignal('sot.forcesNormal')
tr.add('sot.activeForces','')
robot.after.addSignal('sot.activeForces')
# --- CHRONO ------------------------------------------------
class Chrono:
t0=0
def __init__(self):
self.t0 = time.time()
def tic(self):
return time.time()-self.t0
def disptic(self):
print 'tic?'
print self.tic()
chrono=Chrono()
attime(99,chrono.disptic)
# --- RUN ------------------------------------------------
matlab.prec=9
matlab.fullPrec=0
taskCom.controlGain.value = 100
#featureComDes.errorIN.value = (0.06, 0.15, 0.8)
#featureComDes.errorIN.value = (0.06, 0.145, 0.8 )
featureComDes.errorIN.value = (0.1, 0.145, 0.8 )
#Nominal: no zmp overshoot featureComDes.errorIN.value = (0.084, 0.144, 0.804 )
sot.push('taskCom')
#@attime(5)
def rm():
featureComDes.errorIN.value = dyn.com.value
attime(500,stop,'stop')
#go()
# --- DEBUG ----------------------------------------------
'''
for i in range(50): inc()
print sot.velocity.m
print sot.dyndrift.m
print sot.matrixInertia.m
print sot.acceleration.m
print "dq=velocity; b=dyndrift; A=matrixInertia; ddq=acceleration;"
if rs:
print sot.control.m
print sot._LF_f.m
print sot._LF_fn.m
print sot._RF_f.m
print sot._RF_fn.m
print "ddq1=ddq; phil1= _LF_f; phir1 = _RF_f; phi1=[ phil1; phir1 ]; fnl1=_LF_fn; fnr1 = _RF_fn; fn1 = [fnl1; fnr1 ]; tau1=control;"
else:
print sot.sizeForceSpatial.m
print sot.velocity.m
print sot.dyndrift.m
print sot.forceGenerator.m
print sot.Jc.m
print sot.freeMotionBase.m
print sot.freeForceBase.m
print sot.inertiaSqroot.m
print sot.inertiaSqrootInv.m
print sot.driftContact.m
print taskCom.jacobian.m
print taskCom.task.m
print taskCom.Jdot.m
print "X=forceGenerator; V=freeMotionBase; K = freeForceBase; B=inertiaSqrootInv; Bi=inertiaSqroot; Jcom=jacobian; Ab=Bi'*Bi; dc = driftContact; J=X*Jc; G=J*B; Gc=Jc*B; Sb=[eye(6) zeros(6,30)];"
print sot.solution.m
print sot.acceleration.m
sot.forces.recompute(sot.acceleration.time)
print sot.forces.m
print "x=solution; ddq=acceleration; f=forces; phi = X'*f;"
print "u=x(1:24); psi=x(25:end); "
print "ddq2= ddq; phi2=phi; f2=f; fn2=f(3:3:end);"
'''
'''
if 0: # double check
sotreg = SolverOpSpace('sotreg')
sotreg.setSize(robotDim-6)
sotreg.breakFactor.value = 10
plug(dyn.inertiaReal,sotreg.matrixInertia)
plug(dyn.dynamicDrift,sotreg.dyndrift)
plug(dyn.velocity,sotreg.velocity)
sotreg.addContactFromTask(contactLF.task.name,'LF')
sotreg._LF_p.value = contactLF.support
sotreg.addContactFromTask(contactRF.task.name,'RF')
sotreg._RF_p.value = contactRF.support
sotreg.push('taskCom')
sotreg.control.recompute(sot.acceleration.time)
print sotreg.control.m
print sotreg._LF_f.m
print sotreg._LF_fn.m
print sotreg._RF_f.m
print sotreg._RF_fn.m
print sotreg.acceleration.m
print "ddq1=acceleration; phil1= _LF_f; phir1 = _RF_f; phi1=[ phil1; phir1 ]; fnl1=_LF_fn; fnr1 = _RF_fn; fn1 = [fnl1; fnr1 ]; tau1=control;"
print taskCom.task.m
'''
python/jumble/dynwalk.py 0 → 100644
View file @ 60cf95b2
This diff is collapsed.
python/jumble/kinedebug.py 0 → 100644
View file @ 60cf95b2
This diff is collapsed.
python/jumble/kinemorisawa.py 0 → 100644
View file @ 60cf95b2
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 *
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 numpy import *
from matrix_util import matrixToTuple, vectorToTuple,rotate
from history import History
from zmp_estimator import ZmpEstimator
from viewer_helper import addRobotViewer,VisualPinger
from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
robotName = 'hrp14small'
initialConfig.clear()
initialConfig['hrp10small'] = (0,0,0.648697398115,0,0,0)+(0, 0, -0.4538, 0.8727, -0.4189, 0, 0, 0, -0.4538, 0.8727, -0.4189, 0, 0, 0, 0, 0, 0.2618, -0.1745, 0, -0.5236, 0, 0.17453, 0.2618, 0.1745, 0, -0.5236, 0, 0.17453 )
initialConfig['hrp14small'] = (0,0,0.648697398115,0,0,0)+(0, 0, -0.4538, 0.8727, -0.4189, 0, 0, 0, -0.4538, 0.8727, -0.4189, 0, 0, 0, 0, 0, 0.2618, -0.1745, 0, -0.5236, 0, 0, 0.17453, 0.2618, 0.1745, 0, -0.5236, 0, 0, 0.17453 )
# --- ROBOT SIMU ---------------------------------------------------------------
robotDim=robotDimension[robotName]
robot = RobotSimu("robot")
robot.resize(robotDim)
addRobotViewer(robot,small=True,verbose=False)
robot.set( initialConfig[robotName] )
dt=5e-3
# --- MAIN LOOP ------------------------------------------
def inc():
robot.increment(dt)
from ThreadInterruptibleLoop import *
@loopInThread
def loop():
inc()
runner=loop()
@optionalparentheses
def go(): runner.play()
@optionalparentheses
def stop(): runner.pause()
@optionalparentheses
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 ----------------------------------------------------------------------
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()
robot.control.unplug()
# --- PG ---------------------------------------------------------
from dynamic_graph.sot.pattern_generator import PatternGenerator,Selector
pg = PatternGenerator('pg')
pg.setVrmlDir(modelDir+'/')
pg.setVrml(modelName[robotName])
pg.setXmlSpec(specificitiesPath)
pg.setXmlRank(jointRankPath)
pg.buildModel()
# Standard initialization
pg.parseCmd(":samplingperiod 0.005")
pg.parseCmd(":previewcontroltime 1.6")
pg.parseCmd(":comheight 0.814")
pg.parseCmd(":omega 0.0")
pg.parseCmd(":stepheight 0.05")
pg.parseCmd(":singlesupporttime 0.780")
pg.parseCmd(":doublesupporttime 0.020")
pg.parseCmd(":armparameters 0.5")
pg.parseCmd(":LimitsFeasibility 0.0")
pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015")
pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.0 3.0")
pg.parseCmd(":UpperBodyMotionParameters 0.0 -0.5 0.0")
pg.parseCmd(":comheight 0.814")
pg.parseCmd(":SetAlgoForZmpTrajectory Morisawa")
plug(dyn.position,pg.position)
plug(dyn.com,pg.com)
pg.motorcontrol.value = robotDim*(0,)
pg.zmppreviouscontroller.value = (0,0,0)
pg.initState()
# --- PG INIT FRAMES ---
geom = Dynamic("geom")
geom.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath)
geom.parse()
geom.createOpPoint('rf','right-ankle')
geom.createOpPoint('lf','left-ankle')
plug(dyn.position,geom.position)
geom.ffposition.value = 6*(0,)
geom.velocity.value = robotDim*(0,)
geom.acceleration.value = robotDim*(0,)
# --- Selector of Com Ref: when pg is stopped, pg.inprocess becomes 0
comRef = Selector('comRef'
,['vector','ref',dyn.com,pg.comref])
plug(pg.inprocess,comRef.selec)
selecSupportFoot = Selector('selecSupportFoot'
,['matrixHomo','pg_H_sf',pg.rightfootref,pg.leftfootref]
,['matrixHomo','wa_H_sf',geom.rf,geom.lf]
)
plug(pg.SupportFoot,selecSupportFoot.selec)
sf_H_wa = Inverse_of_matrixHomo('sf_H_wa')
plug(selecSupportFoot.wa_H_sf,sf_H_wa.sin)
pg_H_wa = Multiply_of_matrixHomo('pg_H_wa')
plug(selecSupportFoot.pg_H_sf,pg_H_wa.sin1)
plug(sf_H_wa.sout,pg_H_wa.sin2)
# --- Compute the ZMP ref in the Waist reference frame.
wa_H_pg = Inverse_of_matrixHomo('wa_H_pg')
plug(pg_H_wa.sout,wa_H_pg.sin)
wa_zmp = Multiply_matrixHomo_vector('wa_zmp')
plug(wa_H_pg.sout,wa_zmp.sin1)
plug(pg.zmpref,wa_zmp.sin2)
# Connect the ZMPref to OpenHRP in the waist reference frame.
pg.parseCmd(':SetZMPFrame world')
plug(wa_zmp.sout,robot.zmp)
# ---- TASKS -------------------------------------------------------------------
# ---- WAIST TASK ---
taskWaist=MetaTask6d('waist',dyn,'waist','waist')
# Build the reference waist pos homo-matrix from PG.
waistReferenceVector = Stack_of_vector('waistReferenceVector')
plug(pg.initwaistposref,waistReferenceVector.sin1)
plug(pg.initwaistattref,waistReferenceVector.sin2)
waistReferenceVector.selec1(0,3)
waistReferenceVector.selec2(0,3)
waistReference=PoseRollPitchYawToMatrixHomo('waistReference')
plug(waistReferenceVector.sout,waistReference.sin)
plug(waistReference.sout,taskWaist.featureDes.position)
taskWaist.feature.selec.value = '011100'
taskWaist.task.controlGain.value = 5
# --- TASK COM ---
featureCom = FeatureGeneric('featureCom')
plug(dyn.com,featureCom.errorIN)
plug(dyn.Jcom,featureCom.jacobianIN)
featureComDes = FeatureGeneric('featureComDes')
featureCom.sdes.value = 'featureComDes'
plug(comRef.ref,featureComDes.errorIN)
featureCom.selec.value = '011'
taskComPD = Task('taskComPD')
taskComPD.add('featureCom')
#plug(pg.dcomref,featureComDes.errordotIN)
#plug(featureCom.errordot,taskComPD.errorDot)
taskComPD.controlGain.value = 40
#taskComPD.setBeta(-1)
# --- TASK RIGHT FOOT
# Task right hand
taskRF=MetaTask6d('rf',dyn,'rf','right-ankle')
taskLF=MetaTask6d('lf',dyn,'lf','left-ankle')
plug(pg.rightfootref,taskRF.featureDes.position)
taskRF.task.controlGain.value = 5
plug(pg.leftfootref,taskLF.featureDes.position)
taskLF.task.controlGain.value = 5
# ---- 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)
sot.push(taskWaist.task.name)
sot.push(taskRF.task.name)
sot.push(taskLF.task.name)
sot.push(taskComPD.name)
plug(sot.control,robot.control)
# --- TRACER -----------------------------------------------------------------
from dynamic_graph.tracer import *
from dynamic_graph.tracer_real_time import *
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
tr.start()
robot.after.addSignal('tr.triger')
#tr.add(dyn.name+'.ffposition','ff')
tr.add(taskRF.featureDes.name+'.position','refr')
tr.add(taskLF.featureDes.name+'.position','refl')
# --- RUN -----------------------------------------------------------------
def seqC():
# seqpart = "0.2 -0.095 0.0 0.2 0.19 0.0 0.2 -0.19 0.0 0.2 0.19 0.0 0.2 -0.19 0.0 0.2 0.19 0.0 "
seqpart = "0.0 -0.095 0.0 0.2 0.19 0.0 0.0 -0.19 0.0"
pg.parseCmd(":stepseq " + seqpart)
seqC()
go()
python/jumble/romeo/planche.py 0 → 100644
View file @ 60cf95b2
This diff is collapsed.
python/jumble/step.py 0 → 100644
View file @ 60cf95b2
This diff is collapsed.
python/jumble/walktrans.py 0 → 100644
View file @ 60cf95b2
This diff is collapsed.
python/mocap/bigstep.py 0 → 100644
View file @ 60cf95b2
This diff is collapsed.
python/mocap/footprint_parser.py 0 → 100644
View file @ 60cf95b2
This diff is collapsed.
python/mocap/imit.py 0 → 100644
View file @ 60cf95b2
This diff is collapsed.
python/mocap/mocap_parser.py 0 → 100644
View file @ 60cf95b2
This diff is collapsed.
python/mocap/planche.py 0 → 100644
View file @ 60cf95b2
This diff is collapsed.