Beispiel #1
0
 def moveHandModelToFrame(model, frame):
     pos, quat = transformUtils.poseFromTransform(frame)
     rpy = transformUtils.quaternionToRollPitchYaw(quat)
     pose = np.hstack((pos, rpy))
     model.model.setJointPositions(pose, [
         'base_x', 'base_y', 'base_z', 'base_roll', 'base_pitch', 'base_yaw'
     ])
Beispiel #2
0
def convertStateMessageToDrakePose(msg, strict=True):
    '''
    If strict is true, then the state message must contain a joint_position
    for each named joint in the drake pose joint names.  If strict is false,
    then a default value of 0.0 is used to fill joint positions that are
    not specified in the robot state msg argument.
    '''

    jointMap = {}
    for name, position in zip(msg.joint_name, msg.joint_position):
        jointMap[name] = position

    jointPositions = []
    for name in getDrakePoseJointNames()[6:]:
        if strict:
            jointPositions.append(jointMap[name])
        else:
            jointPositions.append(jointMap.get(name, 0.0))

    trans = msg.pose.translation
    quat = msg.pose.rotation
    trans = [trans.x, trans.y, trans.z]
    quat = [quat.w, quat.x, quat.y, quat.z]
    rpy = transformUtils.quaternionToRollPitchYaw(quat)

    pose = np.hstack((trans, rpy, jointPositions))
    assert len(pose) == getNumPositions()
    return pose
Beispiel #3
0
    def generateBoxLinkNode(self, aff):
        link = etree.Element("link", {"name": aff.getDescription()["Name"]})
        link.append(etree.Element("pose"))
        pose = np.append(
            aff.getDescription()["pose"][0],
            transformUtils.quaternionToRollPitchYaw(
                aff.getDescription()["pose"][1]),
        )
        link.find("pose").text = " ".join(map(str, pose))
        dimensions = aff.getDescription()["Dimensions"]

        if aff.getDescription()["Collision Enabled"]:
            link.append(etree.Element("collision", {"name": "collision"}))
            link.find("collision").append(etree.Element("geometry"))
            link.find("collision/geometry").append(etree.Element("box"))
            link.find("collision/geometry/box").append(etree.Element("size"))
            link.find("collision/geometry/box/size").text = " ".join(
                map(str, dimensions))

        link.append(etree.Element("visual", {"name": "visual"}))
        link.find("visual").append(etree.Element("geometry"))
        link.find("visual/geometry").append(etree.Element("box"))
        link.find("visual/geometry/box").append(etree.Element("size"))
        link.find("visual/geometry/box/size").text = " ".join(
            map(str, dimensions))
        link.find("visual").append(etree.Element("material"))

        return link
Beispiel #4
0
    def convertStateMessageToDrakePoseBasic(self,
                                            trans,
                                            quat,
                                            jointNamesIn,
                                            jointPositionsIn,
                                            strict=True):
        """
        If strict is true, then the state message must contain a joint_position
        for each named joint in the drake pose joint names.  If strict is false,
        then a default value of 0.0 is used to fill joint positions that are
        not specified in the robot state msg argument.
        This version is used for ROS messages
        """

        jointMap = {}
        for name, position in zip(jointNamesIn, jointPositionsIn):
            jointMap[name] = position

        jointPositions = []
        for name in self.getStateJointNames():
            if strict:
                jointPositions.append(jointMap[name])
            else:
                jointPositions.append(jointMap.get(name, 0.0))

        rpy = transformUtils.quaternionToRollPitchYaw(quat)

        pose = np.hstack((trans, rpy, jointPositions))
        assert len(pose) == self.getStateLength()
        return pose
Beispiel #5
0
    def generateBoxLinkNode(self, aff):
        link = etree.Element('link', {'name': aff.getDescription()['Name']})
        link.append(etree.Element('pose'))
        pose = np.append(
            aff.getDescription()['pose'][0],
            transformUtils.quaternionToRollPitchYaw(
                aff.getDescription()['pose'][1]))
        link.find('pose').text = ' '.join(map(str, pose))
        dimensions = aff.getDescription()['Dimensions']

        if aff.getDescription()['Collision Enabled']:
            link.append(etree.Element('collision', {'name': 'collision'}))
            link.find('collision').append(etree.Element('geometry'))
            link.find('collision/geometry').append(etree.Element('box'))
            link.find('collision/geometry/box').append(etree.Element('size'))
            link.find('collision/geometry/box/size').text = ' '.join(
                map(str, dimensions))

        link.append(etree.Element('visual', {'name': 'visual'}))
        link.find('visual').append(etree.Element('geometry'))
        link.find('visual/geometry').append(etree.Element('box'))
        link.find('visual/geometry/box').append(etree.Element('size'))
        link.find('visual/geometry/box/size').text = ' '.join(
            map(str, dimensions))
        link.find('visual').append(etree.Element('material'))
        link.find('visual/material').append(etree.Element('diffuse'))
        link.find('visual/material/diffuse').text = '{:s} {:.1f}'.format(
            ' '.join(map(str,
                         aff.getDescription()['Color'])),
            aff.getDescription()['Alpha'])
        link.find('visual/material').append(etree.Element('ambient'))
        link.find('visual/material/ambient').text = '0 0 0 1'

        return link
 def generateCylinderLinkNode(self, aff):
     link = etree.Element('link', {'name':aff.getDescription()['Name']})
     link.append(etree.Element('pose'))
     pose = np.append(aff.getDescription()['pose'][0], transformUtils.quaternionToRollPitchYaw(aff.getDescription()['pose'][1]))
     link.find('pose').text = ' '.join(map(str, pose))
     
     if aff.getDescription()['Collision Enabled']:
         link.append(etree.Element('collision', {'name': 'collision'}))
         link.find('collision').append(etree.Element('geometry'))
         link.find('collision/geometry').append(etree.Element('cylinder'))
         link.find('collision/geometry/cylinder').append(etree.Element('radius'))
         link.find('collision/geometry/cylinder/radius').text = '{:.4f}'.format(aff.getDescription()['Radius'])
         link.find('collision/geometry/cylinder').append(etree.Element('length'))
         link.find('collision/geometry/cylinder/length').text = '{:.4f}'.format(aff.getDescription()['Length'])
     
     link.append(etree.Element('visual', {'name': 'visual'}))
     link.find('visual').append(etree.Element('geometry'))
     link.find('visual/geometry').append(etree.Element('cylinder'))
     link.find('visual/geometry/cylinder').append(etree.Element('radius'))
     link.find('visual/geometry/cylinder/radius').text = '{:.4f}'.format(aff.getDescription()['Radius'])
     link.find('visual/geometry/cylinder').append(etree.Element('length'))
     link.find('visual/geometry/cylinder/length').text = '{:.4f}'.format(aff.getDescription()['Length'])
     link.find('visual').append(etree.Element('material'))
     link.find('visual/material').append(etree.Element('diffuse'))
     link.find('visual/material/diffuse').text = '{:s} {:.1f}'.format(' '.join(map(str, aff.getDescription()['Color'])), aff.getDescription()['Alpha'])
     link.find('visual/material').append(etree.Element('ambient'))
     link.find('visual/material/ambient').text = '0 0 0 1'
     
     return link
 def generateBoxLinkNode(self, aff):
     link = etree.Element('link', {'name':aff.getDescription()['Name']})
     link.append(etree.Element('pose'))
     pose = np.append(aff.getDescription()['pose'][0], transformUtils.quaternionToRollPitchYaw(aff.getDescription()['pose'][1]))
     link.find('pose').text = ' '.join(map(str, pose))
     dimensions = aff.getDescription()['Dimensions']
     
     if aff.getDescription()['Collision Enabled']:
         link.append(etree.Element('collision', {'name': 'collision'}))
         link.find('collision').append(etree.Element('geometry'))
         link.find('collision/geometry').append(etree.Element('box'))
         link.find('collision/geometry/box').append(etree.Element('size'))
         link.find('collision/geometry/box/size').text = ' '.join(map(str, dimensions))
     
     link.append(etree.Element('visual', {'name': 'visual'}))
     link.find('visual').append(etree.Element('geometry'))
     link.find('visual/geometry').append(etree.Element('box'))
     link.find('visual/geometry/box').append(etree.Element('size'))
     link.find('visual/geometry/box/size').text = ' '.join(map(str, dimensions))
     link.find('visual').append(etree.Element('material'))
     link.find('visual/material').append(etree.Element('diffuse'))
     link.find('visual/material/diffuse').text = '{:s} {:.1f}'.format(' '.join(map(str, aff.getDescription()['Color'])), aff.getDescription()['Alpha'])
     link.find('visual/material').append(etree.Element('ambient'))
     link.find('visual/material/ambient').text = '0 0 0 1'
     
     return link
Beispiel #8
0
    def generateBoxLinkNode(self, aff):
        link = etree.Element("link", {"name": aff.getDescription()["Name"]})
        link.append(etree.Element("pose"))
        pose = np.append(
            aff.getDescription()["pose"][0], transformUtils.quaternionToRollPitchYaw(aff.getDescription()["pose"][1])
        )
        link.find("pose").text = " ".join(map(str, pose))
        dimensions = aff.getDescription()["Dimensions"]

        if aff.getDescription()["Collision Enabled"]:
            link.append(etree.Element("collision", {"name": "collision"}))
            link.find("collision").append(etree.Element("geometry"))
            link.find("collision/geometry").append(etree.Element("box"))
            link.find("collision/geometry/box").append(etree.Element("size"))
            link.find("collision/geometry/box/size").text = " ".join(map(str, dimensions))

        link.append(etree.Element("visual", {"name": "visual"}))
        link.find("visual").append(etree.Element("geometry"))
        link.find("visual/geometry").append(etree.Element("box"))
        link.find("visual/geometry/box").append(etree.Element("size"))
        link.find("visual/geometry/box/size").text = " ".join(map(str, dimensions))
        link.find("visual").append(etree.Element("material"))
        link.find("visual/material").append(etree.Element("diffuse"))
        link.find("visual/material/diffuse").text = "{:s} {:.1f}".format(
            " ".join(map(str, aff.getDescription()["Color"])), aff.getDescription()["Alpha"]
        )
        link.find("visual/material").append(etree.Element("ambient"))
        link.find("visual/material/ambient").text = "0 0 0 1"

        return link
Beispiel #9
0
    def generateCylinderLinkNode(self, aff):
        link = etree.Element("link", {"name": aff.getDescription()["Name"]})
        link.append(etree.Element("pose"))
        pose = np.append(
            aff.getDescription()["pose"][0], transformUtils.quaternionToRollPitchYaw(aff.getDescription()["pose"][1])
        )
        link.find("pose").text = " ".join(map(str, pose))

        if aff.getDescription()["Collision Enabled"]:
            link.append(etree.Element("collision", {"name": "collision"}))
            link.find("collision").append(etree.Element("geometry"))
            link.find("collision/geometry").append(etree.Element("cylinder"))
            link.find("collision/geometry/cylinder").append(etree.Element("radius"))
            link.find("collision/geometry/cylinder/radius").text = "{:.4f}".format(aff.getDescription()["Radius"])
            link.find("collision/geometry/cylinder").append(etree.Element("length"))
            link.find("collision/geometry/cylinder/length").text = "{:.4f}".format(aff.getDescription()["Length"])

        link.append(etree.Element("visual", {"name": "visual"}))
        link.find("visual").append(etree.Element("geometry"))
        link.find("visual/geometry").append(etree.Element("cylinder"))
        link.find("visual/geometry/cylinder").append(etree.Element("radius"))
        link.find("visual/geometry/cylinder/radius").text = "{:.4f}".format(aff.getDescription()["Radius"])
        link.find("visual/geometry/cylinder").append(etree.Element("length"))
        link.find("visual/geometry/cylinder/length").text = "{:.4f}".format(aff.getDescription()["Length"])
        link.find("visual").append(etree.Element("material"))
        link.find("visual/material").append(etree.Element("diffuse"))
        link.find("visual/material/diffuse").text = "{:s} {:.1f}".format(
            " ".join(map(str, aff.getDescription()["Color"])), aff.getDescription()["Alpha"]
        )
        link.find("visual/material").append(etree.Element("ambient"))
        link.find("visual/material/ambient").text = "0 0 0 1"

        return link
Beispiel #10
0
def convertStateMessageToDrakePose(msg, strict=True):
    """
    If strict is true, then the state message must contain a joint_position
    for each named joint in the drake pose joint names.  If strict is false,
    then a default value of 0.0 is used to fill joint positions that are
    not specified in the robot state msg argument.
    """

    jointMap = {}
    for name, position in zip(msg.joint_name, msg.joint_position):
        jointMap[name] = position

    jointPositions = []
    for name in getDrakePoseJointNames()[6:]:
        if strict:
            jointPositions.append(jointMap[name])
        else:
            jointPositions.append(jointMap.get(name, 0.0))

    trans = msg.pose.translation
    quat = msg.pose.rotation
    trans = [trans.x, trans.y, trans.z]
    quat = [quat.w, quat.x, quat.y, quat.z]
    rpy = transformUtils.quaternionToRollPitchYaw(quat)

    pose = np.hstack((trans, rpy, jointPositions))
    assert len(pose) == getNumPositions()
    return pose
Beispiel #11
0
def setRobotPoseFromOptitrack(robotToBodyOffset=[0.0, -0.035, 0.0, -90, -90.5, 0], optitrackBodyName='Body 1'):
    bodyToWorld = om.findObjectByName(optitrackBodyName + ' frame').transform
    robotToBody = transformUtils.frameFromPositionAndRPY(robotToBodyOffset[:3], robotToBodyOffset[3:])
    robotToWorld = transformUtils.concatenateTransforms([robotToBody, bodyToWorld])
    pos, quat = transformUtils.poseFromTransform(robotToWorld)
    rpy = transformUtils.quaternionToRollPitchYaw(quat)
    robotSystem.robotStateJointController.q[:6] = np.hstack((pos,rpy))
    robotSystem.robotStateJointController.push()
Beispiel #12
0
 def onPoseBDI(self,msg):
     self.pose_bdi = msg
     # Set the xyzrpy of this pose to equal that estimated by BDI
     rpy = transformUtils.quaternionToRollPitchYaw(msg.orientation)
     pose = self.jointController.q.copy()
     pose[0:3] = msg.pos
     pose[3:6] = rpy
     self.bdiJointController.setPose("ERS BDI", pose)
Beispiel #13
0
def convertStateMessageToDrakePose(msg):

    jointMap = {}
    for name, position in zip(msg.joint_name, msg.joint_position):
        jointMap[name] = position

    jointPositions = []
    for name in getDrakePoseJointNames()[6:]:
        jointPositions.append(jointMap[name])

    trans = msg.pose.translation
    quat = msg.pose.rotation
    trans = [trans.x, trans.y, trans.z]
    quat = [quat.w, quat.x, quat.y, quat.z]
    rpy = transformUtils.quaternionToRollPitchYaw(quat)

    pose = np.hstack((trans, rpy, jointPositions))
    assert len(pose) == getNumPositions()
    return pose
Beispiel #14
0
    def generateCylinderLinkNode(self, aff):
        link = etree.Element('link', {'name': aff.getDescription()['Name']})
        link.append(etree.Element('pose'))
        pose = np.append(
            aff.getDescription()['pose'][0],
            transformUtils.quaternionToRollPitchYaw(
                aff.getDescription()['pose'][1]))
        link.find('pose').text = ' '.join(map(str, pose))

        if aff.getDescription()['Collision Enabled']:
            link.append(etree.Element('collision', {'name': 'collision'}))
            link.find('collision').append(etree.Element('geometry'))
            link.find('collision/geometry').append(etree.Element('cylinder'))
            link.find('collision/geometry/cylinder').append(
                etree.Element('radius'))
            link.find(
                'collision/geometry/cylinder/radius').text = '{:.4f}'.format(
                    aff.getDescription()['Radius'])
            link.find('collision/geometry/cylinder').append(
                etree.Element('length'))
            link.find(
                'collision/geometry/cylinder/length').text = '{:.4f}'.format(
                    aff.getDescription()['Length'])

        link.append(etree.Element('visual', {'name': 'visual'}))
        link.find('visual').append(etree.Element('geometry'))
        link.find('visual/geometry').append(etree.Element('cylinder'))
        link.find('visual/geometry/cylinder').append(etree.Element('radius'))
        link.find('visual/geometry/cylinder/radius').text = '{:.4f}'.format(
            aff.getDescription()['Radius'])
        link.find('visual/geometry/cylinder').append(etree.Element('length'))
        link.find('visual/geometry/cylinder/length').text = '{:.4f}'.format(
            aff.getDescription()['Length'])
        link.find('visual').append(etree.Element('material'))
        link.find('visual/material').append(etree.Element('diffuse'))
        link.find('visual/material/diffuse').text = '{:s} {:.1f}'.format(
            ' '.join(map(str,
                         aff.getDescription()['Color'])),
            aff.getDescription()['Alpha'])
        link.find('visual/material').append(etree.Element('ambient'))
        link.find('visual/material/ambient').text = '0 0 0 1'

        return link
Beispiel #15
0
    def generateCylinderLinkNode(self, aff):
        link = etree.Element("link", {"name": aff.getDescription()["Name"]})
        link.append(etree.Element("pose"))
        pose = np.append(
            aff.getDescription()["pose"][0],
            transformUtils.quaternionToRollPitchYaw(
                aff.getDescription()["pose"][1]),
        )
        link.find("pose").text = " ".join(map(str, pose))

        if aff.getDescription()["Collision Enabled"]:
            link.append(etree.Element("collision", {"name": "collision"}))
            link.find("collision").append(etree.Element("geometry"))
            link.find("collision/geometry").append(etree.Element("cylinder"))
            link.find("collision/geometry/cylinder").append(
                etree.Element("radius"))
            link.find(
                "collision/geometry/cylinder/radius").text = "{:.4f}".format(
                    aff.getDescription()["Radius"])
            link.find("collision/geometry/cylinder").append(
                etree.Element("length"))
            link.find(
                "collision/geometry/cylinder/length").text = "{:.4f}".format(
                    aff.getDescription()["Length"])

        link.append(etree.Element("visual", {"name": "visual"}))
        link.find("visual").append(etree.Element("geometry"))
        link.find("visual/geometry").append(etree.Element("cylinder"))
        link.find("visual/geometry/cylinder").append(etree.Element("radius"))
        link.find("visual/geometry/cylinder/radius").text = "{:.4f}".format(
            aff.getDescription()["Radius"])
        link.find("visual/geometry/cylinder").append(etree.Element("length"))
        link.find("visual/geometry/cylinder/length").text = "{:.4f}".format(
            aff.getDescription()["Length"])
        link.find("visual").append(etree.Element("material"))

        return link
Beispiel #16
0
 def moveHandModelToFrame(model, frame):
     pos, quat = transformUtils.poseFromTransform(frame)
     rpy = transformUtils.quaternionToRollPitchYaw(quat)
     pose = np.hstack((pos, rpy))
     model.model.setJointPositions(pose, ['base_x', 'base_y', 'base_z', 'base_roll', 'base_pitch', 'base_yaw'])
def to_xyzrpy(pose):
    pos = pose[:3]
    quat = pose[6], pose[3], pose[4], pose[5]
    rpy = transformUtils.quaternionToRollPitchYaw(quat)
    return np.hstack([pos, rpy])
Beispiel #18
0
def getRollPitchYawFromRobotState(robotState):
    return transformUtils.quaternionToRollPitchYaw(robotState[3:7])
Beispiel #19
0
def getRollPitchYawFromRobotState(robotState):
    return transformUtils.quaternionToRollPitchYaw(robotState[3:7])