Python quat_to_roll_pitch_yawの例

コード例 #1

ファイル: robotstate.py プロジェクト: rdeits/director

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 = botpy.quat_to_roll_pitch_yaw(quat)

    pose = np.hstack((trans, rpy, jointPositions))
    assert len(pose) == getNumPositions()
    return pose

コード例 #2

ファイル: robotstate.py プロジェクト: rdeits/director

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 = botpy.quat_to_roll_pitch_yaw(quat)

    pose = np.hstack((trans, rpy, jointPositions))
    assert len(pose) == getNumPositions()
    return pose

コード例 #3

ファイル: robotstate.py プロジェクト: rdeits/director

def getRollPitchYawFromRobotState(robotState):
    return botpy.quat_to_roll_pitch_yaw(robotState[3:7])

コード例 #4

ファイル: robotstate.py プロジェクト: rdeits/director

def getRollPitchYawFromRobotState(robotState):
    return botpy.quat_to_roll_pitch_yaw(robotState[3:7])