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() '''