Beispiel #1
0
 def onFrameModified(self, frame):
     pos, rpy = self.frame.transform.GetPosition(
     ), transformUtils.rollPitchYawFromTransform(self.frame.transform)
     q = self.jointController.q.copy()
     q[:3] = pos
     q[3:6] = rpy
     self.jointController.setPose('moved_pose', q)
    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
Beispiel #3
0
def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False):

    obj, _ = vis.findPickedObject(displayPoint, view)

    if not obj:
        return False

    name = obj.getProperty('Name')

    if name in ('footstep widget', 'footstep widget frame'):
        om.removeFromObjectModel(om.findObjectByName('footstep widget'))
        return True

    match = re.match('^step (\d+)$', name)
    if not match:
        return False

    stepIndex = int(match.group(1))

    existingWidget = om.findObjectByName('footstep widget')
    if existingWidget:
        previousStep = existingWidget.stepIndex
        print 'have existing widget for step:', stepIndex

        om.removeFromObjectModel(existingWidget)
        if previousStep == stepIndex:
            print 'returning because widget was for selected step'
            return True


    footMesh = shallowCopy(obj.polyData)
    footFrame = transformUtils.copyFrame(obj.getChildFrame().transform)

    if useHorizontalWidget:
        rpy = [0.0, 0.0, transformUtils.rollPitchYawFromTransform(footFrame)[2]]
        footFrame = transformUtils.frameFromPositionAndRPY(footFrame.GetPosition(), np.degrees(rpy))

    footObj = vis.showPolyData(footMesh, 'footstep widget', parent='planning', alpha=0.2)
    footObj.stepIndex = stepIndex
    frameObj = vis.showFrame(footFrame, 'footstep widget frame', parent=footObj, scale=0.2)
    footObj.actor.SetUserTransform(frameObj.transform)
    footObj.setProperty('Color', obj.getProperty('Color'))
    frameObj.setProperty('Edit', True)

    rep = frameObj.widget.GetRepresentation()
    rep.SetTranslateAxisEnabled(2, False)
    rep.SetRotateAxisEnabled(0, False)
    rep.SetRotateAxisEnabled(1, False)
    frameObj.widget.HandleRotationEnabledOff()

    walkGoal = om.findObjectByName('walking goal')
    if walkGoal:
        walkGoal.setProperty('Edit', False)


    def onFootWidgetChanged(frame):
        footstepsDriver.onStepModified(stepIndex - 1, frame)

    frameObj.connectFrameModified(onFootWidgetChanged)
    return True
Beispiel #4
0
 def initImageRotations(self, robotModel):
     self.robotModel = robotModel
     # Rotate Multisense image/CAMERA_LEFT if the camera frame is rotated (e.g. for Valkyrie)
     if robotModel.getHeadLink():
         tf = robotModel.getLinkFrame(robotModel.getHeadLink())
         roll = transformUtils.rollPitchYawFromTransform(tf)[0]
         if np.isclose(np.abs(roll), np.pi, atol=1e-1):
             self.imageManager.setImageRotation180('CAMERA_LEFT')
Beispiel #5
0
 def initImageRotations(self, robotModel):
     self.robotModel = robotModel
     # Rotate Multisense image/CAMERA_LEFT if the camera frame is rotated (e.g. for Valkyrie)
     if robotModel.getHeadLink():
         tf = robotModel.getLinkFrame(robotModel.getHeadLink())
         roll = transformUtils.rollPitchYawFromTransform(tf)[0]
         if np.isclose(np.abs(roll), np.pi, atol=1e-1):
             self.imageManager.setImageRotation180("CAMERA_LEFT")
Beispiel #6
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
def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False):

    obj, _ = vis.findPickedObject(displayPoint, view)

    if not obj:
        return False

    name = obj.getProperty("Name")

    if name in ("footstep widget", "footstep widget frame"):
        om.removeFromObjectModel(om.findObjectByName("footstep widget"))
        return True

    match = re.match("^step (\d+)$", name)
    if not match:
        return False

    stepIndex = int(match.group(1))

    existingWidget = om.findObjectByName("footstep widget")
    if existingWidget:
        previousStep = existingWidget.stepIndex
        om.removeFromObjectModel(existingWidget)
        if previousStep == stepIndex:
            return True

    footMesh = shallowCopy(obj.polyData)
    footFrame = transformUtils.copyFrame(obj.getChildFrame().transform)

    if useHorizontalWidget:
        rpy = [0.0, 0.0, transformUtils.rollPitchYawFromTransform(footFrame)[2]]
        footFrame = transformUtils.frameFromPositionAndRPY(
            footFrame.GetPosition(), np.degrees(rpy)
        )

    footObj = vis.showPolyData(
        footMesh, "footstep widget", parent="planning", alpha=0.2
    )
    footObj.stepIndex = stepIndex
    frameObj = vis.showFrame(
        footFrame, "footstep widget frame", parent=footObj, scale=0.2
    )
    footObj.actor.SetUserTransform(frameObj.transform)
    footObj.setProperty("Color", obj.getProperty("Color"))
    frameObj.setProperty("Edit", True)

    rep = frameObj.widget.GetRepresentation()
    rep.SetTranslateAxisEnabled(2, False)
    rep.SetRotateAxisEnabled(0, False)
    rep.SetRotateAxisEnabled(1, False)
    frameObj.widget.HandleRotationEnabledOff()

    walkGoal = om.findObjectByName("walking goal")
    if walkGoal:
        walkGoal.setProperty("Edit", False)

    return True
Beispiel #8
0
    def onRobotStateModelChanged(self, model=None):

        if self.lfootFrame is None:
            return

        newPelvisToWorld = self.getPelvisEstimate()

        q = robotStateJointController.q.copy()
        q[0:3] = newPelvisToWorld.GetPosition()
        q[3:6] = transformUtils.rollPitchYawFromTransform(newPelvisToWorld)
        playbackJointController.setPose('lock_pose', q)
Beispiel #9
0
def testEuler():
    '''
    Test some euler conversions
    '''
    quat = transformations.random_quaternion()
    pos = np.random.rand(3)
    frame = transformUtils.transformFromPose(pos, quat)
    mat = transformUtils.getNumpyFromTransform(frame)

    rpy = transformUtils.rollPitchYawFromTransform(frame)
    rpy2 = transformations.euler_from_matrix(mat)

    print rpy
    print rpy2
    assert np.allclose(rpy, rpy2)
Beispiel #10
0
def testEuler():
    '''
    Test some euler conversions
    '''
    quat = transformations.random_quaternion()
    pos = np.random.rand(3)
    frame = transformUtils.transformFromPose(pos, quat)
    mat = transformUtils.getNumpyFromTransform(frame)

    rpy = transformUtils.rollPitchYawFromTransform(frame)
    rpy2 = transformations.euler_from_matrix(mat)

    print(rpy)
    print(rpy2)
    assert np.allclose(rpy, rpy2)
Beispiel #11
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
Beispiel #12
0
def testEulerBotpy():
    '''
    Test some quaternion and euler conversions with botpy
    '''

    quat = transformations.random_quaternion()
    rpy = transformations.euler_from_quaternion(quat)

    rpy2 = botpy.quat_to_roll_pitch_yaw(quat)
    quat2 = botpy.roll_pitch_yaw_to_quat(rpy)

    mat = transformations.quaternion_matrix(quat)
    frame = transformUtils.getTransformFromNumpy(mat)
    rpy3 = transformUtils.rollPitchYawFromTransform(frame)

    print quat, quat2
    print rpy, rpy2, rpy3

    assert isQuatEqual(quat, quat2)
    assert np.allclose(rpy, rpy2)
    assert np.allclose(rpy2, rpy3)
Beispiel #13
0
def testEulerBotpy():
    '''
    Test some quaternion and euler conversions with botpy
    '''

    quat = transformations.random_quaternion()
    rpy = transformations.euler_from_quaternion(quat)

    rpy2 = botpy.quat_to_roll_pitch_yaw(quat)
    quat2 = botpy.roll_pitch_yaw_to_quat(rpy)

    mat = transformations.quaternion_matrix(quat)
    frame = transformUtils.getTransformFromNumpy(mat)
    rpy3 = transformUtils.rollPitchYawFromTransform(frame)

    print quat, quat2
    print rpy, rpy2, rpy3

    assert isQuatEqual(quat, quat2)
    assert np.allclose(rpy, rpy2)
    assert np.allclose(rpy2, rpy3)
    def newDrivingGoal(self, displayPoint, view):
        # Places the driving goal on the plane of the root link current yaw
        # for husky: the bottom of the wheels.
        # for hyq/anymal the midpoint of the trunk
        # TODO: read the link from the director config
        mainLink = drcargs.getRobotConfig(self.robotName)["pelvisLink"]
        footFrame = self.robotModel.getLinkFrame(mainLink)

        worldPt1, worldPt2 = vis.getRayFromDisplayPoint(view, displayPoint)
        groundOrigin = footFrame.GetPosition()
        groundNormal = [0.0, 0.0, 1.0]
        selectedGroundPoint = [0.0, 0.0, 0.0]

        t = vtk.mutable(0.0)
        vtk.vtkPlane.IntersectWithLine(
            worldPt1, worldPt2, groundNormal, groundOrigin, t, selectedGroundPoint
        )

        footFrameRPY = transformUtils.rollPitchYawFromTransform(footFrame)
        drivingTarget = transformUtils.frameFromPositionAndRPY(
            selectedGroundPoint, [0, 0, footFrameRPY[2] * 180.0 / np.pi]
        )

        # Create the widget and send a message:
        # walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
        frameObj = vis.updateFrame(
            drivingTarget, "driving goal", parent="planning", scale=0.25
        )
        frameObj.setProperty("Edit", True)

        rep = frameObj.widget.GetRepresentation()
        rep.SetTranslateAxisEnabled(2, False)
        rep.SetRotateAxisEnabled(0, False)
        rep.SetRotateAxisEnabled(1, False)
        frameObj.widget.HandleRotationEnabledOff()

        frameObj.connectFrameModified(onNewDrivingGoal)
        onNewDrivingGoal(frameObj)
def poseFromAffordance(aff):

    t = aff.getChildFrame().transform
    position, quat = transformUtils.poseFromTransform(t)
    rpy = transformUtils.rollPitchYawFromTransform(t)
    return urdf.Pose(position, rpy)
Beispiel #16
0
def poseFromAffordance(aff):

    t = aff.getChildFrame().transform
    position, quat = transformUtils.poseFromTransform(t)
    rpy = transformUtils.rollPitchYawFromTransform(t)
    return urdf.Pose(position, rpy)
def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False):

    obj, _ = vis.findPickedObject(displayPoint, view)

    if not obj:
        return False

    name = obj.getProperty('Name')

    if name in ('footstep widget', 'footstep widget frame'):
        om.removeFromObjectModel(om.findObjectByName('footstep widget'))
        return True

    match = re.match('^step (\d+)$', name)
    if not match:
        return False

    stepIndex = int(match.group(1))

    existingWidget = om.findObjectByName('footstep widget')
    if existingWidget:
        previousStep = existingWidget.stepIndex
        om.removeFromObjectModel(existingWidget)
        if previousStep == stepIndex:
            return True

    footMesh = shallowCopy(obj.polyData)
    footFrame = transformUtils.copyFrame(obj.getChildFrame().transform)

    if useHorizontalWidget:
        rpy = [
            0.0, 0.0,
            transformUtils.rollPitchYawFromTransform(footFrame)[2]
        ]
        footFrame = transformUtils.frameFromPositionAndRPY(
            footFrame.GetPosition(), np.degrees(rpy))

    footObj = vis.showPolyData(footMesh,
                               'footstep widget',
                               parent='planning',
                               alpha=0.2)
    footObj.stepIndex = stepIndex
    frameObj = vis.showFrame(footFrame,
                             'footstep widget frame',
                             parent=footObj,
                             scale=0.2)
    footObj.actor.SetUserTransform(frameObj.transform)
    footObj.setProperty('Color', obj.getProperty('Color'))
    frameObj.setProperty('Edit', True)

    rep = frameObj.widget.GetRepresentation()
    rep.SetTranslateAxisEnabled(2, False)
    rep.SetRotateAxisEnabled(0, False)
    rep.SetRotateAxisEnabled(1, False)
    frameObj.widget.HandleRotationEnabledOff()

    walkGoal = om.findObjectByName('walking goal')
    if walkGoal:
        walkGoal.setProperty('Edit', False)

    def onFootWidgetChanged(frame):
        footstepsDriver.onStepModified(stepIndex - 1, frame)

    frameObj.connectFrameModified(onFootWidgetChanged)
    return True
Beispiel #18
0
 def onFrameModified(self, frame):
     pos, rpy = self.frame.transform.GetPosition(), transformUtils.rollPitchYawFromTransform(self.frame.transform)
     q = self.jointController.q.copy()
     q[:3] = pos
     q[3:6] = rpy
     self.jointController.setPose('moved_pose', q)