def footUpwards(self, pg, taskPP, opPointRef):
''' Add a task to move the foot to the uppermost part which is halfway to the final
forward position '''
if (opPointRef=='right-ankle'): footPose = self.dyn.rf.value
if (opPointRef=='left-ankle'): footPose = self.dyn.lf.value
dx = (pg.landingfootposition.value[0]+footPose[0][3])/2
dy = (pg.landingfootposition.value[1]+footPose[1][3])/2
dth = (pg.landingfootposition.value[2] + tr2rpy(footPose)[0])/2
goto6dPP(taskPP, (dx, dy, 0.155,0,0,dth), (-0.3,0,0,0,0,0), self.swingT/2, self.sot.solution.time)
def update(self, pg, flag=0):
''' If the pg is currently not working, do not do any update '''
if( not(pg.inprocess.value) ):
return
''' LF leaves floor '''
if( (self.leftfootcontact_prev ==1) & (pg.leftfootcontact.value==0) ):
self.sot.rmContact(self.contact_lf.name)
self.sot.push(self.taskPPlf.task.name)
self.time = 0 # Initialize time as 0
''' Half of the way to go for the foot up '''
dx = (pg.landingfootposition.value[0]+self.dyn.lf.value[0][3])/2
dy = (pg.landingfootposition.value[1]+self.dyn.lf.value[1][3])/2
dth = (pg.landingfootposition.value[2] + tr2rpy(self.dyn.lf.value)[0])/2
goto6dPP(self.taskPPlf, (dx, dy, 0.155,0,0,dth), (-0.3,0,0,0,0,0), self.swingT/2,
self.sot.solution.time)
''' LF is swinging foot '''
elif( (self.leftfootcontact_prev ==0) & (pg.leftfootcontact.value==0) ):
self.time = self.time+1
if(self.time == (138/2)):
p = pg.landingfootposition.value
goto6dPP(self.taskPPlf, (p[0], p[1], 0.105,0,0,p[2]), (0,0,0,0,0,0),
self.swingT/2, self.sot.solution.time)
''' LF returns to floor '''
elif( (self.leftfootcontact_prev ==0) & (pg.leftfootcontact.value==1) ):
self.sot.rm(self.taskPPlf.task.name)
self.sot.addContactFromTask(self.contact_lf.task.name, self.contact_lf.name)
self.sot.signal("_"+self.contact_lf.name+"_p").value = self.contact_lf.support
self.contact_lf.task.resetJacobianDerivative()
if (cmp(self.sot.className,'SolverMotionReduced')==0):
self.sot.signal("_"+self.contact_lf.name+"_ddx3").value = ((0.,0.,0.,),)
''' RF leaves floor '''
if( (self.rightfootcontact_prev ==1) & (pg.rightfootcontact.value==0) ):
self.sot.rmContact(self.contact_rf.name)
self.sot.push(self.taskPPrf.task.name)
self.time = 0
dx = (pg.landingfootposition.value[0]+self.dyn.rf.value[0][3])/2
dy = (pg.landingfootposition.value[1]+self.dyn.rf.value[1][3])/2
dth = (pg.landingfootposition.value[2] + tr2rpy(self.dyn.rf.value)[0])/2
goto6dPP(self.taskPPrf, (dx, dy, 0.155,0,0,dth), (-0.3,0,0,0,0,0), self.swingT/2,
self.sot.solution.time)
''' RF is swinging foot '''
elif( (self.rightfootcontact_prev ==0) & (pg.rightfootcontact.value==0) ):
self.time = self.time+1
if(self.time == (138/2)):
p = pg.landingfootposition.value
goto6dPP(self.taskPPrf, (p[0], p[1], 0.105,0,0,p[2]), (0,0,0,0,0,0),
self.swingT/2, self.sot.solution.time)
''' RF returns to floor '''
elif( (self.rightfootcontact_prev ==0) & (pg.rightfootcontact.value==1) ):
self.sot.rm(self.taskPPrf.task.name)
self.sot.addContactFromTask(self.contact_rf.task.name, self.contact_rf.name)
self.sot.signal("_"+self.contact_rf.name+"_p").value = self.contact_rf.support
self.contact_rf.task.resetJacobianDerivative()
if (cmp(self.sot.className,'SolverMotionReduced')==0):
self.sot.signal("_"+self.contact_rf.name+"_ddx3").value = ((0.,0.,0.,),)
''' Update the previous value with the current value '''
self.leftfootcontact_prev = pg.leftfootcontact.value
self.rightfootcontact_prev = pg.rightfootcontact.value
