def get_link_pQvw()

in bullet/bullet_utils.py [0:0]

• 23 lines of code
• 9 McCabe index (conditional complexity)

def get_link_pQvw(pb_client, body_id, indices=None):
    ''' 
    Returns positions, orientations, linear and angular velocities given link indices.
    Please use get_base_pQvw for the base link.
    ''' 
    if indices is None:
        indices = range(pb_client.getNumJoints(body_id))
    
    num_indices = len(indices)
    assert num_indices > 0
    
    ls = pb_client.getLinkStates(body_id, indices, computeLinkVelocity=True)

    ps = [np.array(ls[j][0]) for j in range(num_indices)]
    if not xyzw_in:
        Qs = [
            quaternion.Q_op(np.array(ls[j][1]), op=['change_order'], xyzw_in=True) \
            for j in range(num_indices)]
    else:
        Qs = [np.array(ls[j][1]) for j in range(num_indices)]
    vs = [np.array(ls[j][6]) for j in range(num_indices)]
    ws = [np.array(ls[j][7]) for j in range(num_indices)]

    if num_indices == 1:
        return ps[0], Qs[0], vs[0], ws[0]
    else:
        return ps, Qs, vs, ws