def drakePoseToQPInput(pose, atlasVersion=5):
    if atlasVersion == 4:
        numPositions = 34
    else:
        numPositions = 36

    msg = lcmt_qp_controller_input()
    msg.timestamp = getUtime()
    msg.num_support_data = 0
    msg.num_tracked_bodies = 0
    msg.num_external_wrenches = 0
    msg.num_joint_pd_overrides = 0
    msg.param_set_name = 'position_control'

    whole_body_data = lcmt_whole_body_data()
    whole_body_data.timestamp = getUtime()
    whole_body_data.num_positions = numPositions
    whole_body_data.q_des = pose
    whole_body_data.num_constrained_dofs = numPositions - 6
    whole_body_data.constrained_dofs = range(7, numPositions+1)
    msg.whole_body_data = whole_body_data

    return msg
dataset/ETHPy150Open RobotLocomotion/director/src/python/tests/testAtlasCommand.py/drakePoseToQPInput/original