sot.breakFactor.value = 10
mp.addJointMap("Rhand", taskrh)
mp.addJointMap("Lhand", tasklh)
mp.addJointMap("head", taskHead)
mp.addJointMap("Rfoot", taskrf)
mp.posture = sot.posture
mp.forward()
taskrf.feature.position.recompute(0)
rf0 = array(taskrf.feature.position.value)[0:3, 3]
sigset = (lambda s, v: s.__class__.value.__set__(s, v))
refset = (lambda mt, v: mt.__class__.ref.__set__(mt, v))
attime(80 * mp.timeScale, (lambda: sot.rmContact("RF"), "Remove RF contact"),
(lambda: sot.push(taskrf.task.name), "Start to track RF mocap"))
attime(250 * mp.timeScale, lambda: sot.push(tasklh.task.name), "Add lf")
attime(700 * mp.timeScale, lambda: sot.rm(taskChest.task.name), 'Remove chest')
attime(780 * mp.timeScale, (lambda: mp.rmJointMap('Lhand'), "keep lh"),
(lambda: mp.rmJointMap('Rhand'), "keep rh"))
attime(830 * mp.timeScale, (lambda: sot.rm(tasklh.task.name), "Rm lh"),
(lambda: sot.rm(taskrh.task.name), "Rm rh"))
attime(900 * mp.timeScale, lambda: sot.push(taskrfr.task.name),
'RF rotation to 0')
attime(960 * mp.timeScale,
mp.addJointMap("Rhand",taskrh)
mp.addJointMap("Lhand",tasklh)
mp.addJointMap("head",taskHead)
mp.addJointMap("Rfoot",taskrf)
mp.posture = sot.posture
mp.forward()
taskrf.feature.position.recompute(0)
rf0 = array(taskrf.feature.position.value)[0:3,3]
sigset = ( lambda s,v : s.__class__.value.__set__(s,v) )
refset = ( lambda mt,v : mt.__class__.ref.__set__(mt,v) )
attime(80*mp.timeScale
,(lambda: sot.rmContact("RF"),"Remove RF contact" )
,(lambda: sot.push(taskrf.task.name), "Start to track RF mocap")
)
attime(250*mp.timeScale
,lambda: sot.push(tasklh.task.name),"Add lf" )
attime(700*mp.timeScale,
lambda: sot.rm(taskChest.task.name), 'Remove chest')
attime(780*mp.timeScale
,(lambda: mp.rmJointMap('Lhand'),"keep lh" )
,(lambda: mp.rmJointMap('Rhand'),"keep rh" ) )
attime(830*mp.timeScale
,(lambda: sot.rm(tasklh.task.name),"Rm lh" )
,(lambda: sot.rm(taskrh.task.name),"Rm rh" ) )
sot.breakFactor.value = 10
featureComDes.errorIN.value = (0.01, 0.09, 0.8077)
gCom.setConstant(30)
gCom.setByPoint(200, 10, 0.005, 0.8)
mp.forward()
# taskrf.feature.selec.value = "101"
sigset = lambda s, v: s.__class__.value.__set__(s, v)
attime(
80 * mp.timeScale,
(lambda: sot.rmContact("RF"), "Remove RF contact"),
(lambda: sot.push(taskrf.task.name), "Start to track RF mocap"),
)
attime(
1000 * mp.timeScale,
(lambda: gCom.setConstant(1), "Lower Com gains"),
(lambda: sigset(featureComDes.errorIN, (0.01, 0, 0.8077)), "Com back to the center"),
)
attime(
1050 * mp.timeScale,
(lambda: goto6d(taskrf, (0.02, -0.12, 0.055)), "RF to final position"),
(lambda: mp.rmJointMap("Rfoot"), "Stop tracking RF mocap"),
)
halfstep = rf0[0:3]+HALFSTEP
step = rf0[0:3]+STEP
lf0 = array(dyn.lf.value)[0:3,3]
comref = (lf0+step)/2
rh0 = array(dyn.rh.value)[0:3,3]
handref = tuple(rh0+HANDSTEP.tolist())
# --- Events ---------------------------------------------
sigset = ( lambda s,v : s.__class__.value.__set__(s,v) )
refset = ( lambda mt,v : mt.__class__.ref.__set__(mt,v) )
attime(2
,(lambda : sot.push(taskCom.task.name),"Add COM")
,(lambda : refset(taskCom, (lf0[0], lf0[1], 0.7)), "Com to left foot")
,(lambda : sot.push(taskrh.task.name),"Add rh")
,(lambda : gotoNd(taskrh,handref,'111') ,"goto rh")
)
attime(125
,(lambda : sot.rmContact('RF'),"rm left contact")
,(lambda : sot.push(taskrf.task.name),'Push RF')
,(lambda : gotoNd(taskrf,halfstep,'111'),'goto halfstep')
)
attime(175, lambda: contact(taskrh,taskrh),'contact rh')
attime(200, lambda: gotoNd(taskrf,step,'111'),'goto step')
attime(225, lambda: goto6d(taskrf,step),'goto6 step')
#comref.value = ( 0.05,0.16,0.7 )
comref.value = ( -0.02,-0.02,0.8 )
#taskRH.feature.selec.value = '111'
taskRH.gain.setConstant(1)
# Hit the Y-border border of the COM
#taskRH.ref = ((0,0,-1,0.22),(0,1,0,-0.64),(1,0,0,1.24),(0,0,0,1))
# Hit both X and Y borders of the COM
taskRH.ref = ((0,0,-1,0.25),(0,1,0,-0.64),(1,0,0,1.24),(0,0,0,1))
#sot.damping.value=.1
sot.addContact(taskLF)
sot.addContact(taskRF)
#sot.push(taskCom.name)
sot.push(taskJL.name)
sot.push(taskSupport.name)
sot.push(taskRH.task.name)
#sot.push(taskCom.name)
#@attime(200)
def freeComZ():
'Free the Z component of the COM'
featureCom.selec.value = '11'
attime(1000,stop,'pause')
attime(1000,dump,'dump')
halfstep = rf0[0:3] + HALFSTEP
step = rf0[0:3] + STEP
lf0 = array(dyn.lf.value)[0:3, 3]
comref = (lf0 + step) / 2
rh0 = array(dyn.rh.value)[0:3, 3]
handref = tuple(rh0 + HANDSTEP.tolist())
# --- Events ---------------------------------------------
sigset = (lambda s, v: s.__class__.value.__set__(s, v))
refset = (lambda mt, v: mt.__class__.ref.__set__(mt, v))
attime(2, (lambda: sot.push(taskCom.task.name), "Add COM"),
(lambda: refset(taskCom, (lf0[0], lf0[1], 0.7)), "Com to left foot"),
(lambda: sot.push(taskrh.task.name), "Add rh"),
(lambda: gotoNd(taskrh, handref, '111'), "goto rh"))
attime(125, (lambda: sot.rmContact('RF'), "rm left contact"),
(lambda: sot.push(taskrf.task.name), 'Push RF'),
(lambda: gotoNd(taskrf, halfstep, '111'), 'goto halfstep'))
attime(175, lambda: contact(taskrh, taskrh), 'contact rh')
attime(200, lambda: gotoNd(taskrf, step, '111'), 'goto step')
attime(225, lambda: goto6d(taskrf, step), 'goto6 step')
attime(380, lambda: contact(contactRF, taskrf), 'contact rf')
attime(80, (lambda: refset(taskCom, tuple(comref.tolist())),
featurePosture.selec.value = toFlags(range(6, 36))
sot.clear()
for task in [taskWaist, taskRF, taskLF]:
task.feature.position.recompute(0)
task.feature.keep()
task.feature.selec.value = '111111'
sot.push(task.task.name)
taskWaist.ref = matrixToTuple(eye(4))
taskWaist.feature.selec.value = '111011'
taskWaist.gain.setByPoint(18, 0.1, 0.005, 0.8)
attime(200, (lambda: sot.rm(taskWaist.task.name), 'Rm waist'),
(lambda: pg.initState(), 'Init PG'),
(lambda: pg.parseCmd(":stepseq " + seqpart), 'Push step list'),
(lambda: sot.push(taskCom.name), 'Push COM'),
(lambda: plug(pg.rightfootref, taskRF.featureDes.position), 'Plug RF'),
(lambda: plug(pg.leftfootref, taskLF.featureDes.position), 'plug LF'))
#attime(700,lambda: sot.push(taskPosture.name),'Add Posture')
#attime(900,lambda: sot.rm(taskCom.name),'rm com')
#attime(1200,lambda: dump(),'dump!')
#attime(1200,lambda: sys.exit(),'Bye bye')
attime(1200, dump, stop)
go()
'''
sot.clear()
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 moveCom2Center():
featureComDes.errorIN.value = (0.01356, 0.001535, 0.8077)
#-----------------------------------------------------------------------------
# --- RUN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
t = TimeControl()
sot.clear()
#sot.push('taskLim')
gCom.setByPoint(35.0, 5.0, 0.02, 0.5)
#gCom.setByPoint( 150.0, 50.0, 0.02, 0.5)
attime(1, pushCom, moveCom2Left)
attime(2000, raiseRightFoot)
attime(6000, lowerRightFoot)
attime(9000, addRightContact)
attime(9001, moveCom2Center)
attime(11500, stop)
#inc()
go()
'''
matlab.prec=9
matlab.fullPrec=0
print sot.velocity.m
print sot.dyndrift.m
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)
ql = matrix(dyn.lowerJl.value)
ql[0,15] = MAX_SUPPORT_EXT
taskLim.referencePosInf.value = vectorToTuple(ql)
sot.push(taskLim.name)
plug(robot.state,sot.position)
q0 = robot.state.value
rf0 = matrix(taskrf.feature.position.value)[0:3,3]
# --- Events ---------------------------------------------
sigset = ( lambda s,v : s.__class__.value.__set__(s,v) )
refset = ( lambda mt,v : mt.__class__.ref.__set__(mt,v) )
attime(2
,(lambda : sot.push(taskCom.task.name),"Add COM")
,(lambda : refset(taskCom, ( 0.01, 0.09, 0.7 )), "Com to left foot")
)
attime(140
,(lambda: sot.rmContact("RF"),"Remove RF contact" )
,(lambda: sot.push(taskrf.task.name), "Add RF")
,(lambda: gotoNd(taskrf, (0,0,0.65),"000110" ), "Up foot RF")
)
# Was -.8,0,.8 with full extension
attime(500,lambda: gotoNd(taskrf, (-0.75,0,0.7),"000101" ), "Extend RF")
attime(800,lambda: sot.push(taskChest.task.name), "add chest")
attime(1000
,(lambda: sot.rm(taskrf.task.name), "rm RF")
mp.addJointMap("Rfoot", taskrf)
mp.posture = sot.posture
plug(robot.state, sot.position)
sot.breakFactor.value = 10
featureComDes.errorIN.value = (0.01, 0.09, 0.8077)
gCom.setConstant(30)
gCom.setByPoint(200, 10, 0.005, 0.8)
mp.forward()
#taskrf.feature.selec.value = "101"
sigset = (lambda s, v: s.__class__.value.__set__(s, v))
attime(80 * mp.timeScale, (lambda: sot.rmContact("RF"), "Remove RF contact"),
(lambda: sot.push(taskrf.task.name), "Start to track RF mocap"))
attime(1000 * mp.timeScale, (lambda: gCom.setConstant(1), "Lower Com gains"),
(lambda: sigset(featureComDes.errorIN,
(0.01, 0, 0.8077)), "Com back to the center"))
attime(1050 * mp.timeScale,
(lambda: goto6d(taskrf, (0.02, -0.12, 0.055)), "RF to final position"),
(lambda: mp.rmJointMap("Rfoot"), "Stop tracking RF mocap"))
attime(1100 * mp.timeScale, lambda: contact(contactRF, taskrf),
"Add the RF contact")
attime(3000, stop)
m()
def moveCom2Center():
featureComDes.errorIN.value = (0.01356, 0.001535, 0.8077 )
#-----------------------------------------------------------------------------
# --- RUN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
t = TimeControl()
sot.clear()
#sot.push('taskLim')
gCom.setByPoint( 35.0, 5.0, 0.02, 0.5)
#gCom.setByPoint( 150.0, 50.0, 0.02, 0.5)
attime( 1, pushCom, moveCom2Left)
attime( 2000, raiseRightFoot)
attime( 6000, lowerRightFoot)
attime( 9000, addRightContact)
attime( 9001, moveCom2Center)
attime( 11500, stop)
#inc()
go()
'''
matlab.prec=9
matlab.fullPrec=0
q0 = robot.state.value
com0 = dyn.com.value
rh0 = dyn.rh.value
sot.push(taskLim.name)
plug(robot.state,sot.position)
sot.breakFactor.value = 2
# --- Events ---------------------------------------------
sigset = ( lambda s,v : s.__class__.value.__set__(s,v) )
refset = ( lambda mt,v : mt.__class__.ref.__set__(mt,v) )
attime(2
,(lambda : sot.push(taskCom.task.name),"Add COM-X")
,(lambda : sot.push(taskSupport.name),"Add COM-Y")
,(lambda : refset(taskCom, com0), "Com to rest")
,(lambda: sot.push(taskrh.task.name), "Add RH")
,(lambda: sot.push(tasklh.task.name), "Add LH")
)
attime(400
,(lambda: gotoNd(taskrh,(),'110111'),'RH to 6D')
,(lambda: taskrh.feature.keep(),'keep RH ')
,(lambda: gotoNd(tasklh,(),'110111'),'LH to 6D')
,(lambda: tasklh.feature.keep(),'keep LH ')
,(lambda: sot.push(taskWaist.task.name),'Add waist')
,(lambda: gotoNd(taskWaist,(0,0.0,SQ[1,0]),'110'),'Waist to up')
,(lambda: sigset(sot.posture,dyn.position.value), "Robot to keep pose")
)
attime(500,lambda: gotoNd(taskWaist,(0,SQ[0,0],SQ[1,0]),'110'),'Waist to left')
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)
taskrfz.feature.frame('desired')
taskHead.feature.selec.value='011000'
taskHead.featureDes.position.value = matrixToTuple(eye(4))
sot.push(taskLim.name)
sot.push(taskHead.task.name)
plug(robot.state,sot.position)
# --- Events ---------------------------------------------
sigset = ( lambda s,v : s.__class__.value.__set__(s,v) )
refset = ( lambda mt,v : mt.__class__.ref.__set__(mt,v) )
attime(2
,(lambda : sot.push(taskCom.task.name),"Add COM")
,(lambda : sigset(taskCom.feature.selec, "11"),"COM XY")
,(lambda : refset(taskCom, ( 0.01, 0.09, 0.7 )), "Com to left foot")
)
attime(140
,(lambda: sot.rmContact("RF"),"Remove RF contact" )
,(lambda: sot.push(taskrfz.task.name), "Add RFZ")
,(lambda: sot.push(taskrf.task.name), "Add RF")
,(lambda: gotoNd(taskrfz,rf0,"11100" ), "Up foot RF")
)
attime(150 ,lambda : sigset(taskCom.feature.selec, "11"),"COM XY")
attime(200 ,
lambda: gotoNd(taskrf, rf0+(0.35,0.0,0),"11") , "RF to front" )
# --- 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)
sot.clear()
for task in [taskWaist, taskRF, taskLF]:
task.feature.position.recompute(0)
task.feature.keep()
task.feature.selec.value = '111111'
sot.push(task.task.name)
taskWaist.ref = matrixToTuple(eye(4))
taskWaist.feature.selec.value = '111011'
taskWaist.gain.setByPoint(18,0.1,0.005,0.8)
attime(200
,(lambda : sot.rm(taskWaist.task.name),'Rm waist')
,(lambda : pg.initState(),'Init PG')
,(lambda : pg.parseCmd(":stepseq " + seqpart),'Push step list')
,(lambda : sot.push(taskCom.name),'Push COM')
,(lambda : plug(pg.rightfootref,taskRF.featureDes.position),'Plug RF')
,(lambda : plug(pg.leftfootref,taskLF.featureDes.position),'plug LF' )
)
#attime(700,lambda: sot.push(taskPosture.name),'Add Posture')
#attime(900,lambda: sot.rm(taskCom.name),'rm com')
#attime(1200,lambda: dump(),'dump!')
#attime(1200,lambda: sys.exit(),'Bye bye')
attime(1200,dump,stop)
go()
'''
