#from contact_handler import ContactSelectPassingPoint
from contact_handler_new import ContactSelectPassingPointMotionReduced
#from contact_handler_2013 import ContactSelectPassingPointMotionReduced
contactSelect = ContactSelectPassingPointMotionReduced(sot, dyn)
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.after.addSignal(taskLim.task.name+'.normalizedPosition')
#-----------------------------------------------------------------------------
# --- RUN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
sot.clear()
contact(contactLF)
contact(contactRF)
''' To avoid problems with solverMotionReduced '''
if (cmp(sot.className,'SolverMotionReduced')==0):
sot._RF_ddx3.value = ((0.,0.,0.,),)
sot._LF_ddx3.value = ((0.,0.,0.,),)
sot.push(taskLim.task.name)
plug(robot.state,sot.position)
q0 = robot.state.value
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")
# )
