def fwd_kinematics(self, manip_name, dof_map=None, mat_result=False):
    if (dof_map is not None):
        self.set_dof(dof_map)
    trans = self.env_body.GetLink(manip_name).GetTransform()
    if mat_result:
        return trans
    pos = trans[:3, 3]
    quat = quatFromRotationMatrix(trans[:3, :3])
    return {'pos': pos, 'quat': quat}
def fwd_kinematics(self, manip_name, dof_map=None, mat_result=False):
    if (dof_map is not None):
        self.set_dof(dof_map)
    trans = self.env_body.GetLink(manip_name).GetTransform()
    if mat_result:
        return trans
    pos = trans[:3, 3]
    quat = quatFromRotationMatrix(trans[:3, :3])
    return {'pos': pos, 'quat': quat}