コード例 #1
0
 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)
コード例 #2
0
 def footDownwards(self, pg, taskPP):
     ''' Add a task to move the foot to the final position specified by the pattern generator '''
     p = pg.landingfootposition.value
     goto6dPP(taskPP, (p[0], p[1], 0.105,0,0,p[2]), (0,0,0,0,0,0), self.swingT/2, self.sot.solution.time)
コード例 #3
0
ファイル: contact_handler.py プロジェクト: Oscar-R/sot-oscar
    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