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 ---
