Beispiel #1
0
    def setupTasks(self, opSpaceController):
        robot = self.robot

        feet = robotinfo.feet(robot)

        # priority 1
        # right foot task -- stay still relative to left foot
        RFTask = opSpaceController.addTask(LinkTask(robot, feet['r'], "po",
                                                    feet['l']),
                                           priority=1,
                                           name="RF",
                                           gains=(0.0, 0, 0))

        # priority 2
        # CoM task
        self.comTask = opSpaceController.addTask(COMTask(robot),
                                                 priority=2,
                                                 gains=([-60.0, -30.0, -10.0],
                                                        [-0.4, -0.4,
                                                         -0.2], 0.0),
                                                 weight=[1.0, 1.0, 1.0])

        #correct for com errors only using legs and hip
        self.comTask.activeDofs = robotinfo.legsAndBase(robot)
        #self.comTask.vcmdmax = 0.25
        #approximation to ZMP criterion
        #self.comTask.dvcmdmax = 100.0

        #store local position of COM relative to feet
        self.comLocalHomePos = {}
        for k, v in feet.iteritems():
            self.comLocalHomePos[v] = se3.apply(
                se3.inv(robot.link(v).getTransform()),
                self.comTask.getDesiredValue())
        """
Beispiel #2
0
    def setupTasks(self,opSpaceController):
        robot = self.robot

        feet = robotinfo.feet(robot)
                
        # priority 1
        # right foot task -- stay still relative to left foot
        RFTask = opSpaceController.addTask(LinkTask(robot, feet['r'], "po", feet['l']),
                                           priority=1,
                                           name="RF",
                                           gains=(0.0,0,0))
        
        # priority 2
        # CoM task
        self.comTask = opSpaceController.addTask(COMTask(robot),
                                                 priority=2,
                                                 gains=([-60.0,-30.0,-10.0], [-0.4,-0.4,-0.2], 0.0),
                                                 weight=[1.0,1.0,1.0])

        #correct for com errors only using legs and hip
        self.comTask.activeDofs = robotinfo.legsAndBase(robot)
        #self.comTask.vcmdmax = 0.25
        #approximation to ZMP criterion
        #self.comTask.dvcmdmax = 100.0

        #store local position of COM relative to feet
        self.comLocalHomePos = {}
        for k,v in feet.iteritems():
            self.comLocalHomePos[v] = se3.apply(se3.inv(robot.link(v).getTransform()),self.comTask.getDesiredValue())

        """
Beispiel #3
0
 def output_and_advance(self, **inputs):
     lf = inputs['LF_ForceSensor']
     rf = inputs['RF_ForceSensor']
     #print lf,rf
     out = {}
     contactLinks = []
     if self.lfscale * lf[2] > self.lfthresh:
         out['lfcontact'] = 1
         contactLinks.append(robotinfo.feet(self.robot)['l'])
     else:
         out['lfcontact'] = 0
     if self.rfscale * rf[2] > self.rfthresh:
         out['rfcontact'] = 1
         contactLinks.append(robotinfo.feet(self.robot)['r'])
     else:
         out['rfcontact'] = 0
     print "Estimated links in contact", contactLinks
     out['contactLinks'] = contactLinks
     return out
Beispiel #4
0
 def output_and_advance(self,**inputs):
     lf = inputs['LF_ForceSensor']
     rf = inputs['RF_ForceSensor']
     #print lf,rf
     out = {}
     contactLinks = []
     if self.lfscale*lf[2] > self.lfthresh:
         out['lfcontact']=1
         contactLinks.append(robotinfo.feet(self.robot)['l'])
     else:
         out['lfcontact']=0
     if self.rfscale*rf[2] > self.rfthresh:
         out['rfcontact']=1
         contactLinks.append(robotinfo.feet(self.robot)['r'])
     else:
         out['rfcontact']=0
     print "Estimated links in contact",contactLinks
     out['contactLinks'] = contactLinks
     return out
Beispiel #5
0
    def __init__(self,robot,jointStiffness=0,jointRecovery=1):
        self.jointStiffness = jointStiffness
        self.jointRecovery = jointRecovery
        OpSpaceController.__init__(self,robot)

        # set the motion model to assume that the feet are fixed in place
        assert(robotinfo.freeBase(robot)==True)
        feet = robotinfo.feet(robot)
        assert len(feet)==2
        self.setMotionModel(FreeBaseRobotMotionModel('velocity','velocity',robot,feet.values()))
Beispiel #6
0
    def __init__(self, robot, jointStiffness=0, jointRecovery=1):
        self.jointStiffness = jointStiffness
        self.jointRecovery = jointRecovery
        OpSpaceController.__init__(self, robot)

        # set the motion model to assume that the feet are fixed in place
        assert (robotinfo.freeBase(robot) == True)
        feet = robotinfo.feet(robot)
        assert len(feet) == 2
        self.setMotionModel(
            FreeBaseRobotMotionModel('velocity', 'velocity', robot,
                                     feet.values()))