def getFeetMidPoint(model, useWorldZ=True):
        '''
        Returns a frame in world coordinate system that is the average of the left
        and right foot reference point positions in world frame, the average of the
        left and right foot yaw in world frame, and Z axis aligned with world Z.
        The foot reference point is the average of the foot contact points in the foot frame.
        '''
        contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts()

        contact_pts_mid_left = np.mean(contact_pts_left, axis=0) # mid point on foot relative to foot frame
        contact_pts_mid_right = np.mean(contact_pts_right, axis=0) # mid point on foot relative to foot frame

        t_lf_mid = model.getLinkFrame(_leftFootLink)
        t_lf_mid.PreMultiply()
        t_lf_mid.Translate(contact_pts_mid_left)

        t_rf_mid = model.getLinkFrame(_rightFootLink)
        t_rf_mid.PreMultiply()
        t_rf_mid.Translate(contact_pts_mid_right)

        if "atlas" in _modelName: # atlas_v3/v4/v5
            t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5)
        elif (_modelName == "valkyrie"): # valkyrie
            t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5)
        else:
            raise ValueError("Model Name not recognised")

        if useWorldZ:
            rpy = [0.0, 0.0, np.degrees(transformUtils.rollPitchYawFromTransform(t_feet_mid)[2])]
            return transformUtils.frameFromPositionAndRPY(t_feet_mid.GetPosition(), rpy)
        else:
            return t_feet_mid
示例#2
0
        def interpolateSplineFrame(u):

            assert 0.0 <= u <= 1.0

            pt = [0.0, 0.0, 0.0]
            self.splineWidget.GetRepresentation().GetParametricSpline(
            ).Evaluate([u, 0.0, 0.0], pt, range(9))

            handleParameterization = self.computeHandleParameterization()

            if u >= handleParameterization[self.rotationEndIndex]:
                uu = 1.0

            elif u <= handleParameterization[self.rotationStartIndex]:
                uu = 0.0
            else:
                uu = (u - handleParameterization[self.rotationStartIndex]) / (
                    handleParameterization[self.rotationEndIndex] -
                    handleParameterization[self.rotationStartIndex])

            #print 'rescaled u:', u, '-->', uu

            t = transformUtils.frameInterpolate(palmFrame, goalFrame, uu)
            t.PostMultiply()
            t.Translate(np.array(pt) - np.array(t.GetPosition()))

            return t
示例#3
0
    def getFeetMidPoint(self, useWorldZ=True):
        """
        Returns a frame in world coordinate system that is the average of the left
        and right foot reference point positions in world frame, the average of the
        left and right foot yaw in world frame, and Z axis aligned with world Z.
        The foot reference point is the average of the foot contact points in the foot frame.
        """
        if self.quadruped:
            t_lf = np.array(self.getLinkFrame(self.leftHandLink).GetPosition())
            t_rf = np.array(
                self.getLinkFrame(self.rightHandLink).GetPosition())
            t_lh = np.array(self.getLinkFrame(self.leftFootLink).GetPosition())
            t_rh = np.array(
                self.getLinkFrame(self.rightFootLink).GetPosition())
            mid = (t_lf + t_rf + t_lh + t_rh) / 4
            # this is not optimal, correct approach should use contact points to
            # determine desired orientation, not the current orientation
            rpy = [
                0.0, 0.0,
                self.getLinkFrame(self.pelvisLink).GetOrientation()[2]
            ]
            return transformUtils.frameFromPositionAndRPY(mid, rpy)

        contact_pts_left, contact_pts_right = self.getContactPts()

        contact_pts_mid_left = np.mean(
            contact_pts_left,
            axis=0)  # mid point on foot relative to foot frame
        contact_pts_mid_right = np.mean(
            contact_pts_right,
            axis=0)  # mid point on foot relative to foot frame

        t_lf_mid = self.getLinkFrame(self.leftFootLink)
        t_lf_mid.PreMultiply()
        t_lf_mid.Translate(contact_pts_mid_left)

        t_rf_mid = self.getLinkFrame(self.rightFootLink)
        t_rf_mid.PreMultiply()
        t_rf_mid.Translate(contact_pts_mid_right)

        if self._modelName == "anymal":  # anymal (not used)
            t_feet_mid = transformUtils.frameInterpolate(
                t_lf_mid, t_rf_mid, 0.5)
        else:
            raise ValueError("Model Name not recognised")

        if useWorldZ:
            rpy = [
                0.0,
                0.0,
                np.degrees(
                    transformUtils.rollPitchYawFromTransform(t_feet_mid)[2]),
            ]
            return transformUtils.frameFromPositionAndRPY(
                t_feet_mid.GetPosition(), rpy)
        else:
            return t_feet_mid
示例#4
0
    def getPelvisEstimate(self):

        p = robotStateModel.getLinkFrame('pelvis')
        lf = robotStateModel.getLinkFrame('l_foot')
        rf = robotStateModel.getLinkFrame('r_foot')

        pelvisLeft = self.getPelvisEstimateFromFoot(p, lf, self.lfootFrame)
        pelvisRight = self.getPelvisEstimateFromFoot(p, rf, self.rfootFrame)

        return transformUtils.frameInterpolate(pelvisLeft, pelvisRight, self.footBias)
示例#5
0
    def getFeetMidPoint(model, useWorldZ=True):
        '''
        Returns a frame in world coordinate system that is the average of the left
        and right foot reference point positions in world frame, the average of the
        left and right foot yaw in world frame, and Z axis aligned with world Z.
        The foot reference point is the average of the foot contact points in the foot frame.
        '''
        contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts()

        contact_pts_mid_left = np.mean(contact_pts_left, axis=0) # mid point on foot relative to foot frame
        contact_pts_mid_right = np.mean(contact_pts_right, axis=0) # mid point on foot relative to foot frame

        t_lf_mid = model.getLinkFrame(_leftFootLink)
        t_lf_mid.PreMultiply()
        t_lf_mid.Translate(contact_pts_mid_left)

        t_rf_mid = model.getLinkFrame(_rightFootLink)
        t_rf_mid.PreMultiply()
        t_rf_mid.Translate(contact_pts_mid_right)

        if (_robotType == 0): # Atlas
            t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5)
        elif (_robotType == 1):
            # Valkyrie v1 Foot orientation is silly
            l_foot_sole = transformUtils.frameFromPositionAndRPY([0,0,0], [180.0, 82.5, 0])
            l_foot_sole.PostMultiply()
            l_foot_sole.Concatenate(t_lf_mid)
            r_foot_sole = transformUtils.frameFromPositionAndRPY([0,0,0], [0.0, -82.5, 0])
            r_foot_sole.PostMultiply()
            r_foot_sole.Concatenate(t_rf_mid)
            t_feet_mid = transformUtils.frameInterpolate(l_foot_sole, r_foot_sole, 0.5)
        elif (_robotType == 2):
            # Valkyrie v2 is better
            t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5)


        if useWorldZ:
            rpy = [0.0, 0.0, np.degrees(transformUtils.rollPitchYawFromTransform(t_feet_mid)[2])]
            return transformUtils.frameFromPositionAndRPY(t_feet_mid.GetPosition(), rpy)
        else:
            return t_feet_mid
示例#6
0
    def planRoomSweep(self):
        self.initConstraintSet()

        faceFrameDesired = transformUtils.frameInterpolate(
            self.startFrame.transform, self.endFrame.transform, 0)
        vis.showFrame(faceFrameDesired,
                      'frame 0',
                      visible=True,
                      scale=0.1,
                      parent=self.mapFolder)
        self.addConstraintForTargetFrame(faceFrameDesired, 0)

        faceFrameDesired = transformUtils.frameInterpolate(
            self.startFrame.transform, self.endFrame.transform, 1.0 / 3.0)
        vis.showFrame(faceFrameDesired,
                      'frame 1',
                      visible=True,
                      scale=0.1,
                      parent=self.mapFolder)
        self.addConstraintForTargetFrame(faceFrameDesired, 1)

        faceFrameDesired = transformUtils.frameInterpolate(
            self.startFrame.transform, self.endFrame.transform, 2.0 / 3.0)
        vis.showFrame(faceFrameDesired,
                      'frame 2',
                      visible=True,
                      scale=0.1,
                      parent=self.mapFolder)
        self.addConstraintForTargetFrame(faceFrameDesired, 2)

        faceFrameDesired = transformUtils.frameInterpolate(
            self.startFrame.transform, self.endFrame.transform, 3.0 / 3.0)
        vis.showFrame(faceFrameDesired,
                      'frame 3',
                      visible=True,
                      scale=0.1,
                      parent=self.mapFolder)
        self.addConstraintForTargetFrame(faceFrameDesired, 3)

        #self.ikPlanner.ikServer.maxDegreesPerSecond = self.speedLow
        self.planTrajectory()
示例#7
0
    def planRoomSweep(self):
        self.initConstraintSet()

        faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 0)
        vis.showFrame(faceFrameDesired, 'frame 0', visible=True, scale=0.1,parent=self.mapFolder)
        self.addConstraintForTargetFrame(faceFrameDesired, 0)

        faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 1.0/3.0)
        vis.showFrame(faceFrameDesired, 'frame 1', visible=True, scale=0.1,parent=self.mapFolder)
        self.addConstraintForTargetFrame(faceFrameDesired, 1)

        faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 2.0/3.0)
        vis.showFrame(faceFrameDesired, 'frame 2', visible=True, scale=0.1,parent=self.mapFolder)
        self.addConstraintForTargetFrame(faceFrameDesired, 2)

        faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 3.0/3.0)
        vis.showFrame(faceFrameDesired, 'frame 3', visible=True, scale=0.1,parent=self.mapFolder)
        self.addConstraintForTargetFrame(faceFrameDesired, 3)

        #self.ikPlanner.ikServer.maxDegreesPerSecond = self.speedLow
        self.planTrajectory()
示例#8
0
        def interpolateSplineFrame(u):

            assert 0.0 <= u <= 1.0

            pt = [0.0, 0.0, 0.0]
            self.splineWidget.GetRepresentation().GetParametricSpline().Evaluate([u,0.0,0.0], pt, range(9))

            handleParameterization = self.computeHandleParameterization()

            if u >= handleParameterization[self.rotationEndIndex]:
                uu = 1.0

            elif u <= handleParameterization[self.rotationStartIndex]:
                uu = 0.0
            else:
                uu = (u - handleParameterization[self.rotationStartIndex]) / (handleParameterization[self.rotationEndIndex] - handleParameterization[self.rotationStartIndex])

            #print 'rescaled u:', u, '-->', uu

            t = transformUtils.frameInterpolate(palmFrame, goalFrame, uu)
            t.PostMultiply()
            t.Translate(np.array(pt) - np.array(t.GetPosition()))

            return t