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)
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)
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 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 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 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 now(t=None): if t==None: t=robot.state.time attime(t+1,clearAttimePeriodic) attime(t+50,goForTheBall)
dyn.rh.recompute(0) lh0 = tuple([x[3] for x in dyn.lh.value]) rh0 = tuple([x[3] for x in dyn.rh.value]) r = radians sot.clear() contact(contactLF) contact(contactRF) sot.push(taskLim.task.name) plug(robot.state,sot.position) attime(2 ,(lambda: sot.push(taskChest.task.name), "Add Chest to the SoT") ,(lambda: taskChest.feature.keep(), "Keep Chest") ,(lambda: sot.push(taskHead.task.name), "Add Head to the SoT") ,(lambda: gotoNd(taskHead, (0, 0, 0, 0, r(25), -r(40)), "111000"), "Rotate Head to the right") ,(lambda: sot.push(taskrh.task.name), "Add Right hand to the SoT") ,(lambda: gotoNd(taskrh, (rh0[0]-0.3, rh0[1]-0.4, 0), "000011"), "Move right hand") ) attime(200 ,(lambda: gotoNd( taskHead, (0, 0, 0, 0, r(25), r(40)), "111000"), "Rotate Head to the left") ,(lambda: sot.push(tasklh.task.name), "Add Left hand to the SoT") ,(lambda: gotoNd(tasklh, (lh0[0]+0.3, lh0[1]+0.4, 0), "000011"), "Move left hand") ) attime(400 ,(lambda: gotoNd(taskHead, (0, 0, 0, 0, 0, 0), "111000"), "Rotate Head to the center") ,(lambda: gotoNd(tasklh, (lh0[0], lh0[1], 0), "000011"), "Return left hand to initial position") ,(lambda: gotoNd(taskrh, (rh0[0], rh0[1], 0), "000011"), "Return right hand to initial position") )
#sot.push(taskLim.name) plug(robot.device.state,sot.position) q0 = robot.device.state.value rf0 = matrix(taskrf.feature.position.value)[0:3,3] MetaTaskDynPosture.postureRange = postureRange['romeo'] # --- Events --------------------------------------------- # The sequence of tasks. 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") ) attime(500,lambda: gotoNd(taskrf, (-0.55,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") ,(lambda: sot.push(taskPosture.task.name), "add posture")
chest0 = matrix(dyn.chest.value)[0:3,3] taskCom.feature.selec.value = "11" taskCom.gain.setByPoint(100,10,0.005,0.8) r = radians 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(2 , (lambda: sot.push(taskChest.task.name), "Add Chest"), (lambda: taskChest.feature.keep(), "Keep Chest"), (lambda: sot.push(taskHead.task.name), "Add Head"), (lambda: gotoNd(taskHead, (0, 0, 0, 0, r(25), -r(40)), "111000"), "Rotate Head to the right"), ) attime(200, (lambda: gotoNd( taskHead, (0, 0, 0, 0, r(25), r(40)), "111000"), "Rotate Head to the left") ) attime(400 , (lambda: gotoNd(taskHead, (0, 0, 0, 0, 0, 0), "111000"), "Rotate Head to the center") ) attime(600, stop, "Stopped") go()
lh0 = tuple([x[3] for x in dyn.lh.value]) rh0 = tuple([x[3] for x in dyn.rh.value]) r = radians sot.clear() contact(contactLF) contact(contactRF) sot.push(taskLim.task.name) plug(robot.state, sot.position) attime( 2, (lambda: sot.push(taskChest.task.name), "Add Chest to the SoT"), (lambda: taskChest.feature.keep(), "Keep Chest"), (lambda: sot.push(taskHead.task.name), "Add Head to the SoT"), (lambda: gotoNd(taskHead, (0, 0, 0, 0, r(25), -r(40)), "111000"), "Rotate Head to the right"), (lambda: sot.push(taskrh.task.name), "Add Right hand to the SoT"), (lambda: gotoNd(taskrh, (rh0[0] - 0.3, rh0[1] - 0.4, 0), "000011"), "Move right hand"), ) attime( 200, (lambda: gotoNd(taskHead, (0, 0, 0, 0, r(25), r(40)), "111000"), "Rotate Head to the left"), (lambda: sot.push(tasklh.task.name), "Add Left hand to the SoT"), (lambda: gotoNd(tasklh, (lh0[0] + 0.3, lh0[1] + 0.4, 0), "000011"), "Move left hand"), ) attime( 400, (lambda: gotoNd(taskHead, (0, 0, 0, 0, 0, 0), "111000"), "Rotate Head to the center"),
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()
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'
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] MetaTaskDynPosture.postureRange = postureRange[robotName] # --- 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.55, 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"), (lambda: sot.push(taskPosture.task.name), "add posture"), (lambda: taskPosture.gotoq( chest=(0, ), rleg=(0, 0, 0, MAX_EXT, 0, 0), head=(0, 0, 0, 0)), "extend body"))
def clearAttimePeriodic(): '''Stop the PG.''' del attime.events[ALWAYS] attime(ALWAYS,trackTheBall) pg.pg.velocitydes.value = (0,0,0)
def clearAttimePeriodic(): '''Stop the PG.''' del attime.events[ALWAYS] attime(ALWAYS, trackTheBall) pg.pg.velocitydes.value = (0, 0, 0)
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)
def now(t=None): if t == None: t = robot.state.time attime(t + 1, clearAttimePeriodic) attime(t + 50, goForTheBall)
contactSelect.setContactTasks(contactRF, contactLF) contactSelect.setFreeSpaceTasks(taskPPrf, taskPPlf) contactSelect.setCollisionDetection(fclRf, fclLf) contactSelect.setRobot(robot) contact(contactRF) contact(contactLF) #sot.push(taskLim.name) sot.push(taskCom.task.name) sot.push(taskWaist.task.name) sigset = ( lambda s,v : s.__class__.value.__set__(s,v) ) # --- Events ------------------------------------------------------------- attime(1000 #,(lambda: sigset(pg.velocitydes, (0.0,0.0,0.0) ) , "" ) ,(lambda: sigset(pg.velocitydes, (0.1,0.0,0.5) ) , "Change velocity of pattern" ) ) attime(2000 ,(lambda: sigset(pg.velocitydes, (0.0,0.0,0.0) ) , "Stop walking generation (velocity=0)" ) ) robot.before.addSignal('fclRf.contactPoints') robot.before.addSignal('fclLf.contactPoints')
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 ---